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Abstract 

This Paper explores the use of an extended Kalman filter to provide real-time esti- 
mates of underwater vehicle position and attitude. The types of previously available sen- 
sors are detailed including strapdovvn accelerometers, roll and pitch sensors, gyro and 
magnetic compasses, depth sensor, and various types of acoustic positioning systems. A 
doppler velocimeter is added to this sensor suite to improve the performance of the filter. 
As an integral part of the filter, magnetic compass and gyrocompass biases are estimated 
to improve vehicle heading accuracy. The filter is designed to account for numerous real- 
life complications. These include varying rates of sensor output, lengthy gaps in reception 
of position information, presence of non-Gaussian position fix errors (flyers), and varying 
probability density functions for sensor errors. Simulated data are used to test the filter 
with varying availability of data and accuracy of initial conditions, along with actual data 
from a deployment of the towed DSL- 120 vehicle. The increased accuracy obtained by 
using the doppler velocimeter is emphasized. 
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Chapter 1 

Introduction 



1.1 Motivation 

This thesis has two major goals. The first is to provide accurate estimates of 
underwater vehicle position and attitude and thereby improve the quality of sonar and 
video information collected. The second is to provide these estimates in real-time. 

1.1.1 Improving Vehicle Position and Attitude Information 

More than two thirds of the Eanh is covered by water. Until very recently in 
history, the ocean depths were mysterious and inaccessible. However, in the past few 
decades new technologies have dramatically expanded man’s abilities to explore the ocean 
bottom. Discoveries range from the remnants of human incursions, epitomized by the 
Titanic , to new life on the ocean floor gaining its sustenance from the earth’s interior 
rather than the sun. 

One of the primary tools for conducting underwater searches and surveys from 
unmanned vehicles is the sidescan sonar. The sonar emits a beam of relatively high- 
frequency sound on either side of the vehicle. When this sound beam hits the bottom or 
any objects within range, it is reflected and the return is received by transducers mounted 
on the vehicle. The characteristics of the received sound, primarily magnitude and phase, 
can then be analyzed to determine the characteristics and contours of the bottom as well as 
the location and general shape of any objects. 

For this system to work at an optimum level, it is vital that the location and attitude 
of the vehicle be known as accurately as possible. Any position errors lead directly and 
obviously to an error in the assumed position of all objects mapped by the sonar. Equally 
important, however, is knowledge of the vehicle’s roll, pitch, and heading. Due to the 
nature of sound propagation in water, a significant time lag occurs between the 
transmission of a sonar ping and the receipt of any returns. During this time, the vehicle 
will not only have changed position but may have changed its attitude as well. For proper 
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interpretation of the sonar returns, the attitude of the vehicle at the time of sonar 
transmission must be known, because that determines the precise direction of the beam. 
This same information is also needed at the time of the sonar return, so the location of any 
objects found can be accurately determined. 

1.1.2 Previous Work on Underwater Vehicle Navigation 

The invention of feasible inertial navigation produced a great interest in combining 
inertial measurements with other fix sources to improve navigational accuracy. These 
early inertial navigation units required gimbal-stabilized platforms to achieve the desired 
accuracy, which added size and weight. Due to these restrictions, early work was confined 
to aircraft and ships, including submarines. In the early 1970s, the U.S. Air Force was one 
of the first to evaluate the use of the Kalman filter to integrate inertial measurements with 
external fix sources ( D’Appolito , 1971). 

Later, strapdown inertial systems permitted much smaller and lighter equipment, 
though at a cost in accuracy. Once again, the military provided a major impetus to 
developing these systems to their fullest potential. For example, the North Atlantic Treaty 
Organization’s (NATO) Advisory Group for Aerospace Research and Development 
(AGARD) included much work on these strapdown systems, although still for use 
primarily in aircraft and spacecraft ( VanBronkhorst , 1978; Caiforcl , 1978). 

Ever since remotely operated vehicles (ROVs) became feasible, there has been 
interest in improving their navigational accuracy as well. Early ROVs had only a few 
sensors available to provide navigational information, since inertial navigational systems 
were still too heavy and power-intensive to be usable. These included a magnetic compass 
to provide heading and inclinometers to measure pitch and roll. Additionally, an acoustic 
transponder network was often used to provide position information for operations in a 
small area. A later improvement was the gyrocompass, which provides a more consistent 
output than the magnetic compass but adds new errors caused by drift. 

Improvements in the information provided by these acoustic transponder nets has 
been marked. In her 1992 thesis, Diane Di Massa provided one example with her 
exploration of the possibilities of hyperbolic navigation to permit long-range acoustic 
navigation by an Autonomous Underwater Vehicle (AUV) ( Di Massa , 1992). Brian 



Tracey contributed another with his improvements to a newer technique for ROV 
navigation, ultrashort-baseline navigation, in his 1992 thesis ( Tracey , 1992). 

In the past few years, a number of new devices have been developed to assist in 
solving this navigational problem. In his 1988 thesis, Gregory Vaughn discussed the use of 
a lightweight, strapdown inertial motion unit in conjunction with an external acoustic 
positioning net to improve closed-loop vehicle conmol ( Vaughn , 1988). He concluded that 
by incorporating this new data into a Kalman filter to provide real-time optimal estimates 
of vehicle parameters, closed-loop control of ROVs, such as the Woods Hole 
Oceanographic Institution’s Jason, could be improved. However, for his application 
vehicle attitude was unimportant and therefore was not incorporated into his simulations. 

A fairly recent addition to the repertoire of instruments available to the 
oceanographic engineer is the doppler velocimeter, also known as the acoustic doppler 
current profiler (ADCP). For the first time, accurate real-time velocity measurements of a 
ROV could be made relatively cheaply and without exceeding the restrictive power and 
weight limits inherent in ROV operations. RD Instruments, a doppler velocimeter 
manufacturer, has been funded by the Office of Naval Research to explore the use of the 
instrument to provide an independent and accurate measurement of vehicle velocity 
within the context of a Kalman filter. Their initial work verifies the utility of the 
velocimeter, although so far their research has been confined to inertial navigation systems 
too large for ROV use (Rowe and Bnnnley, 1992). 

These are but a few examples of the extensive work that has been published on 
improving underwater vehicle navigation. A search of the literature reveals diverse 
techniques for improving sensor information and integration, including several variations 
on Kalman filtering techniques. 

1.1.3 Cost and Processing Time Reduction 

In the real world, cost considerations are almost always important. Cruises are 
expensive and usually result in computer disks full of unprocessed, raw data, which are 
essentially worthless until processed. This processing can take many man hours, which 
translates into a considerable cost. Unfortunately, this sometimes means that processing is 
never completed, since either the time or the money runs out. Therefore, the more real- 



time processing that can be performed the better. Real-time processing means that the 
important data are immediately available. Also, since it is accomplished at sea while the 
cruise is in progress (and already paid for), additional expenses are minimized. 

Another important application for this capability pertains to autonomous 
underwater vehicles (AUVs). Much research is being conducted currently on these 
vehicles, which should be capable of operating independently for days or even weeks. 
Real-time processing is essential to permit the vehicle to navigate successfully and is also 
vital in reducing the on-board data storage requirements of AUV deployments. 

1.1.4 Resources Available 

This thesis investigates how the navigational sensors currently available at the 
Deep Submergence Laboratory (DSL) can be used to improve the real-time position and 
attitude information available for the operation of a ROV. This summer, DSL personnel 
participated in a cruise aboard the R/V Knorr. A major purpose of the cruise was to obtain 
detailed sidescan sonar data from an area of the Mid-Atlantic Ridge where seafloor 
spreading is occurring. The DSL-120, a towed vehicle carrying a 120-kHz sidescan sonar 
system and multiple navigational sensors, was used for this survey. Data from these DSL- 
120 deployments are used to demonstrate the performance of the Kalman filter developed 
in this thesis. By integrating the velocity data provided by the doppler velocimeter into a 
Kalman filter algorithm, the vehicle’s navigational accuracy can be greatly enhanced. This 
substantially improves the usefulness of the sonar data collected and demonstrates the 
feasibility of performing real-time navigation sensor processing. 



1.2 Advantages and Limitations of the Kalman Filter 

Sonar data collected by underwater vehicles has often been rendered nearly useless 
due to inaccurate navigation information and an overly simplistic approach to tracking 
vehicle parameters. For example, relying solely on a magnetic compass for heading 
information nearly guarantees that the measured heading will be inaccurate. 

The Kalman filter uses a state space model to provide a well defined method for 
integrating the information obtained from different sensors into a coherent whole. Taking 
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advantage of the a priori knowledge of the relative accuracies of these sensors as well as 
the dynamics of the system, the Kalman filter can provide real-time output without 
overstressing computer memory or computational limits. 

The addition of the doppler velocimeter provides a marked improvement in the 
ability of a Kalman filter to provide valid position and attitude information. By providing 
accurate measurements of the magnitude of vehicle velocities in all three dimensions, a 
more precise, continuous estimate of vehicle position and attitude can be provided than 
would be available by the other sensors alone. 

In this application, there are several obstacles to overcome in creating a usable 
Kalman filter. First, the system is nonlinear and cannot therefore use the simplest and most 
mathematically precise version of Kalman filter theory. Instead, an extended Kalman filter 
is required. Second, a method for ignoring obviously faulty measurements must be 
included. Finally, there are several practical problems involved in using the output of such 
a wide variety of sensors. These problems and their proposed solutions are discussed in 
detail later in this thesis. 



1.3 Thesis Outline 

Chapter Two describes the sensors used to provide information for the Kalman 
filter. Principles of operation for each sensor are discussed, as well as limitations and 
considerations for each when integrated into the overall system. 

Chapter Three explains the development of the Kalman filter and its state space 
model for this application. A basic review of Kalman filter theory is included, with 
emphasis on the specifics of the extended Kalman filter required for nonlinear problems. 
This chapter also discusses the specific problems that were addressed tor proper filter 
operation and how the filter was implemented. 

Chapter Four shows the results of filter operation by using both simulated and 
actual data. The improvement in performance obtained by using the doppler velocimeter 
measurements of vehicle velocities is emphasized. 



14 



The results are summarized in Chapter Five, along with suggestions for further 
refinements to the filter. Finally, some possibilities for additional experiments are 
discussed. 



\ 
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Chapter 2 

Sensor Information 



2.1 Overview of Sensors 

Like any mathematical abstraction of a real-world system, the Kalman filter is a 
simplified model of reality. For the filter to function optimally, the model must be as 
complete and as accurate as possible. For this application there are two major aspects of 
the filter that must be considered. The first concerns prediction: based on knowledge of 
present system parameters, what will happen to these parameters in the future? The second 
is more basic: how can knowledge of these system parameters be obtained? 

In this chapter, the latter issue is addressed. Each sensor providing input to the 
filter is described in detail. Using our knowledge of the available sensors, the Kalman 
filter model can then be developed. Before proceeding further, however, a clarification of 
the coordinate systems used is required. 



2.2 Coordinate Systems 

Within the filter algorithm, two different coordinate systems are used. The first is 
earth-referenced, with positions measured relative to fixed reference points on the earth. 
The second is vehicle-referenced, also referred to as body-referenced. In this case, all 
measurements are made relative to a specific point on the ROV, usually the center of 
rotation of the vehicle. Table 2-1 and Figure 2-1 explain the differences in more detail. 

An additional coordinate system is sensor-referenced, which depends on the 
location of the individual sensor. For linear acceleration and velocity measurements, a 
difference in measurement magnitudes between sensor-referenced and vehicle-referenced 
measurements exists due to the effects of angular velocities when the sensor is mounted 
away from the vehicle’s center of rotation. The orientation of the axes is the same as 
vehicle-referenced coordinates since the instruments are mounted colinear with the 
vehicle axes. Compensation for this difference is discussed in Section 3.5.3. 
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Figure 2-1. Vehicle-referenced coordinate system. 



Table 2-1: Comparison of Coordinate Systems 



Direction 


Earth- Referenced 


Vehicle-Referenced 


X 


Longitude (+X = east) 


Perpendicular to side of 
vehicle (+X = to starboard) 


Y 


Latitude (+Y = north) 


In direction of vehicle bow 
or stern (+Y = forward) 


Z 


Depth/Altitude (+Z = up) 


Perpendicular to top or bot- 
tom of vehicle (+Z = up) 



2.3 Doppler Velocimeter 

The proper name for this instrument, used to provide vehicle velocities, is the 
Direct-Reading Broadband Acoustic Doppler Current Profiler (DR-BBADCP). 
Manufactured by RD Instruments (RDI), the DR-BBADCP is designed to measure current 
velocities at discrete points through the water column (see Fig. 2-2). Its alternate mode, 
which is the mode used to gather data for this application, is the bottom-track profiling 
mode. With the DR-BBADCP mounted on the vehicle looking downward, vehicle 
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Figure 2-2. Broadband ADCP setup for current profiling ( RDI , 1993). 



instrument in this application, the more descriptive name of “doppler velocimeter” is used 
in the remainder of this thesis. 

2.3.1 Principles of Operation 

The doppler velocimeter uses four downward-looking acoustic beams operating at 
high frequency. Figure 2-3 shows the beam pattern of two of these beams in an upward- 
looking mode. RDI manufactures these instruments with six different transmit 
frequencies: 75, 150, 300, 600, 1200, and 2400 kHz. As with any sonar, increasing 
frequency improves the accuracy but reduces the effective range. Therefore, the frequency 
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CURRENT VECTOR 




Fig. 2-3. ADCP beam geometry ( RDI , 1993). 

for a given operation is chosen to be as high as possible while still providing sufficient 
range to ensonify the bottom based on the expected altitude of the vehicle. 

As evidenced by its name, the instrument operates on the doppler principle. 
Vehicle velocity in the direction of a beam increases the frequency of the returned signal, 
while velocity away from the beam decreases it. Specifically, the vehicle velocity in the 
direction of the beam is calculated by (RDI, 1993). 



Relative Flow Velocity (m/s) = F ^ 




where 

is the measured doppler frequency shift in kHz, 
c is the speed of sound in water at the transducer face in m/s, 
F$ is the transmitted acoustic frequency in kHz. 
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Fig. 2-4. Frequency change caused by doppler effect (RDI, 1993). 



Figure 2-4 shows this effect with a fixed transducer and moving material in the 
water. The effect is the same with the transducer mounted on a moving vehicle over the 
fixed bottom. The four beams transmit at different angles so the velocities in each 
direction can be resolved. Three are needed to solve for velocities in all three directions 
and the fourth provides redundancy to ensure accuracy. 

To interpret the received sonar data, the doppler velocimeter uses an 
autocorrelation processor. A series of short pulses is transmitted, each with a pulse length 
of Tp with a known lag T L between pulses. When the returns are received, the processor 
compares the phase change between two distinct pulses, accounting for the difference in 
time between their respective transmissions. As shown in Fig. 2-5, a zero phase change 
implies zero velocity. Likewise, a phase change equates to a velocity with magnitude 
determined by the amount of phase change. Phase changes with a magnitude greater than 
27t are resolved using a proprietary RDI algorithm using different subsets of the - 
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Fig. 2-5. Phase change seen by the broadband system ( RDI , 1993). 



transmitted pulses. If any datum is below the operator-selector minimum for correlation 
magnitude, that datum is rejected by the doppler velocimeter’s processor. 

The output of the velocimeter can be referenced four different ways. The most 
basic provides measurements of the velocities relative to each of the four beams. 
Alternatively, the velocimeter’s internal processor can resolve the components of the four 
beams to provide velocity fore or aft, velocity left or right, and velocity up or down 
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relative to the instrument. Third, if the instrument is not mounted on a ship or ROV with 
the same orientation as the platform’s reference axes, the velocities can be converted to 
any desired direction relative to the platform using knowledge of the difference between 
the instrument axes and platform axes. Finally, the doppler velocimeter can use its internal 
flux-gate sensor, which uses heading and pendulum sensors to provide pitch and roll. The 
internal processor can use this roll, pitch, and heading information or similar data from 
external sensors to convert from vehicle-referenced to earth-referenced velocities. 

For this Kalman filter the velocimeter is mounted with axes oriented the same as 
the vehicle axes described in Section 2.2. Therefore, a coordinate transformation to the 
platform axes is unnecessary except for any correction due to angular velocities. Instead of 
allowing the velocimeter’s internal processor to convert velocities to earth-referenced 
coordinates, which would use the current output of the appropriate sensor for attitude 
measurements, the filter uses the vehicle-referenced values. Using vehicle-referenced 
velocities allows the filtered estimates of roll, pitch, and heading to be used to make the 
conversion to earth-referenced coordinates. This improves overall filter performance since 
the filtered estimates are less noisy than the instantaneous sensor outputs. 

2.3.2 Inputs to Velocimeter 

To provide the desired output, the doppler velocimeter uses the speed of sound in 
its calculations of vehicle velocity. As is the case for data used in this thesis, this speed of 
sound is typically entered manually by the operator to permit easier post-processing of the 
data. However, for more accurate calculations the doppler velocimeter can calculate the 
current speed of sound using three inputs: salinity, depth, and temperature. 

For most open-ocean operations salinity is constant. Therefore, salinity can be 

manually inserted prior to operations. If operations are to be conducted in an area of 

varying salinity, such as near a river mouth, then an external conductivity sensor can be 
* 

used to provide salinity information to the velocimeter. 

Since the majority of vehicle operations occur within a relatively narrow depth 
band, making speed of sound changes due to depth change negligible, depth is also 
normally manually inserted. If operations involving large depth changes are expected. 



22 



consideration should be given to providing depth estimates to the velocimeter to calculate 
a variable speed of sound. 

Finally, the doppler velocimeter has an internal temperature sensor. Manual input 
can be used in this case as well. For operations near the bottom in deep water, where 
thennal gradients are negligible, manually inputting temperature permits more precise 
post-processing of the data without introducing significant errors. In areas with larger 
gradients, using the installed temperature sensor is necessary. 

2.3.3 Doppler Velocimeter Outputs 

Obviously, the primary output desired from the doppler velocimeter is vehicle- 
referenced velocity. However, there are several operator-selected options that affect the 
accuracy and the frequency of these measurements. 

Frequency of output is determined by three factors: the speed of sound in the 
water, the range to the bottom (vehicle altitude), and the number of pings per ensemble. 
The doppler velocimeter averages the results from the pings in each ensemble to improve 
accuracy. The technical manual recommends four pings per ensemble. A larger number of 
pings per ensemble results in better quality measurements but at the cost of a lower update 
rate. Using the recommended four pings per ensemble, an average altitude of 100m, and 
the nominal speed of sound in seawater of 1500 m/s, the time between velocity 
measurements can be calculated as 

4 pings X 200 m/ping X 1 s/1500 m = 0.53 s/ensemble. 

The standard deviation for velocity measurements is computed by the 
velocimeter’s processor based on the BBADCP frequency (for the model in use), the 
range to the bottom, the number of pings per ensemble, and the size ot the depth cells 
selected prior to operation. The size of the depth cell is important tor precision when 
operating in the water column profiling mode. For bottom tracking, however, the largest 
depth cell should always be used since this gives the lowest standard deviation for vehicle 
velocity measurements. For vehicle operations treated in this thesis, the standard deviation 
is on the order of 1-2 cm/s. Figure 2-6 shows how accuracy varies with range for a single 
ping using the 150-kHz model. This high accuracy enables precise measurement of 



23 




Fig. 2-6. Standard deviation for a single ping using the 150-kHz system (with ADCP 
velocity, from top to bottom, of 20 knots, 10 knots, and 0 knots) ( RDl , 1993). 

velocity magnitude. However, overall system accuracy is limited by how well heading, 
pitch, and roll can be estimated. Since standard deviation is constantly updated by the 
doppler velocimeter’s software, that information can be included in the Kalman filter 
algorithm. Specifics of this implementation are discussed in Chapter 3. 



2.4 Depth Sensor 

The depth sensor used for this work is manufactured by Paroscientific, Inc. and 
consists of a quartz-crystal resonator whose frequency of oscillation varies with pressure- 
induced stress. The sensor also includes thermal compensation using a quartz-crystal 
temperature signal. Nominal accuracy of the sensor is 0.02%, and a reading can be 
obtained approximately every 0.25s. ( Paroscientific , 1987). 

For the purposes of achieving the best overall estimates of vehicle position and 
velocity, the absolute error of this sensor is less important than its stability. Previous 
experience indicates that the depth sensor should be very consistent in its outputs. 
Therefore, the output of the sensor is a vital element in increasing the accuracy of the 
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Kalman filter used to determine state space estimates for depth-dependent vehicle 
parameters. 

The only time the sensor should exhibit less stability than desired is if the sensor is 
not in thermal equilibrium with the surrounding water. This could occur soon after initial 
deployment. However, in this case the problem would be foreseen, and thermal 
equilibrium could be achieved prior to commencing actual search operations. Of more 
concern is operation in water with strong thermal gradients. This problem could be 
particularly acute in shallow-water operations, especially at certain times of the day. 
During such operations, the pressure sensor can be expected to give less accurate results, 
resulting in greater Kalman filter errors. 



2.5 External Positioning System 

There are several possible systems that can be used to provide external position 
information. Historically, nearly all underwater vehicle positioning information has been 
obtained from some type of acoustic net. These acoustic systems generally fall into three 
different categories, with the type used for a specific operation dependent on the type of 
mission and the topography of the bottom. 

2.5.1 Long-Baseline Systems 

In a long-baseline system, an array of acoustic transponders is deployed on the 
bottom in the vicinity of projected vehicle operations. A transponder is also attached to the 
vehicle. Vehicle position is determined by measuring the travel times of sound waves 
between the vehicle and the transponders in the net. For two-way systems, the vehicle 
sends out a ping, and each transponder responds with a coded return signal when it 
receives the initial ping from the vehicle. The travel times are then translated into range. 
One-way systems use the same principle, but travel time is measured only from the 
vehicle to each transponder in the net. With either system, since the positions of the 
transponders on the bottom have already been determined when calibrating the acoustic 
net, the position of the vehicle can be determined (Morgan, 1978). 



An example of such a system is the Sonic High Accuracy Ranging and Positioning 
System (SHARPS). This commercial system uses three transceivers near the ocean 
bottom, which are connected to a surface ship by coaxial cables. Another transceiver is 
mounted on the ROV, which is also connected to the ship. Because the cable connections 
allow the surface ship to know the exact transmission time of each ping, only one-way 
travel times are needed, which allows twice the frequency of updates as a two-way 
system. Operating at 300 kHz, SHARPS is limited to a range of 100 m in seawater but can 
provide accuracies on the order of 2 cm ( Somers , 1991). 

A disadvantage of SHARPS is the need for multiple cables from the surface ship to 
the transponder field, which can cause difficulties when maneuvering a ROV in the area. 
Another system, EXACT, was used by Yoerger and Mindell to control the ROV Jason in 
the vicinity of an active hydrothermal plume at a depth of 2200 meters. The EXACT 
system operates similarly to SHARPS and provides comparable accuracies but uses no 
cable connections to the surface. Instead, the master unit on the ROV communicates with 
the surface through a low-bandwidth serial link. Because there are no cable connections, 
two-way travel times are used (Yoerger and Mindell , 1992). 

The major advantages of a long-baseline system are its stability and its accuracy 
when conducting vehicle operations in deep water close to the bottom. Because the 
transponder array is on the seafloor, it is fixed once placed and calibrated. Therefore, its 
accuracy is improved over a less stable system. Since the array of transponders is 
deployed on the bottom near the area of operations, the difference in acoustic travel times 
between each pinger and the vehicle will be greater than if all pingers were located close 
to the surface, which also enhances accuracy. Finally, the acoustic net can be made as 
large as desired. By using lower frequency systems, the maximum range to the ROV can 
be greatly increased at the cost of reduced accuracy and decreased update frequency. 

There are two major disadvantages of this method. First, because a transponder 
net must be deployed on the bottom and calibrated, significant preparation time is required 
before ROV operations can begin. Second, vehicle operations are restricted to the vicinity 
of the acoustic transponders. Therefore, tor large-area surveys, this method is impractical. 
In this case, a short-baseline system may be used ( Morgan , 1978). 
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2.5.2 Short-Baseline Systems 

The principles of operation of the short-baseline systems are similar to those 
having a long baseline. In fact, a system such as SHARPS can be used in either 
configuration. The obvious difference is that instead of the acoustic net transponders being 
placed on the ocean floor, they are attached to the surface ship. This method eliminates 
some disadvantages of the long-baseline system. Since the transponders are mounted on 
the surface vessel in known locations, no initial surveying is required. Also, the ship can 
be moved wherever operations are desired, so the range limitations of a bottom-fixed net 
are negated. 

Like the long-baseline system, the short-baseline system measures the difference 
in arrival time from the acoustic pinger on the ROV to each transponder on the ship. 
However, because the geometry of the ship-mounted array is precisely known, this 
difference in arrival times can be converted into an angular direction from the plane of the 
array to the ROV. Since the ROV is tethered to the ship, the time of each ping is also 
known precisely. Therefore, both bearing and range information are available, yielding the 
location of the ROV (Morgan, 1978). 

However, the short-baseline system tends to increase position errors because of the 
more sensitive geometry. Also, because the transponder array is now mounted on a 
moving platform, surface positioning errors can become the dominant factor in 
determining vehicle location. The importance of accurately measuring the surface ship’s 
heading, pitch, and roll also become vital, especially when the range to the ROV 
increases, since errors in these measurements result in an erroneous measurement of 
bearing to the ROV (Somers, 1991). 

2.5.3 Ultrashort-Baseline Systems 

The ultrashort-baseline system uses a hydrophone array mounted either on the 
ROV or the surface ship, with only a single pinger on the other platform. Normally, the 
hydrophone array is on the surface ship. Phase comparisons rather than travel-time 
measurements are used to determine the position ot the vehicle. The acoustic pinger sends 
out a pulse, and the processor connected to the hydrophone array measures the phase 
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difference in the signals received at each hydrophone. This phase difference is converted 
into a bearing to the pinger. For tethered vehicles, the time of travel can also be computed 
as in the short-baseline case to obtain range. Therefore, ROV position can be determined. 
To measure phase difference accurately, the receiving hydrophones are mounted within 
one wavelength of each other (typically, only centimeters apart), making the array small 
enough for use on ROVs. 

This system has two major disadvantages. First, the installation must be extremely 
precise, which can be difficult. Second, multipath arrivals of the sound waves via bottom- 
bounce or surface-reflection can cause false phase measurements resulting in large 
inaccuracies. In his 1992 thesis, Brian Tracey explores techniques to minimize the effects 
of this multipath interference ( Tracey , 1992). 

2.5.4 Other Positioning Systems 

Besides traditional acoustic positioning systems, other possibilities for 
determining vehicle position exist. For example, research is being performed at DSL and 
other laboratories in making terrain-relative mapping feasible. This method entails 
matching bottom features seen by the vehicle with known features from an existing 
database, or with features already found and mapped by the vehicle. Depending on the 
features present in the vicinity, the quality of the database, and such system characteristics 
as sonar frequency, position fixes with accuracies on the order of meters are foreseeable. 

2.5.5 Position Information Required for Use in the Kalman Filter 

For the purposes of constructing the filter, any of these system configurations can 
be used, but allowances must be made to account for the differences between them. A 
longer range system, using lower frequencies, is typically less accurate. Additionally, 
since the frequency of fixes is limited by the speed of sound in the water, the longer range 
system will obtain fixes less frequently. The filter must be adjusted accordingly. 
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2.6 Gyro 

The gyro used at DSL is manufactured by Humphrey, Inc. It is a standard 
uncompensated model that provides continuous heading information. It maintains 
excellent short-term precision with little “jitter,” or high-frequency fluctuation. However, 
because it is uncompensated, it has a high drift rate that tends to fluctuate from three to six 
degrees per hour. This drift introduces a significant and variable bias that can have a 
debilitating effect on the Kalman filter, which assumes only zero-mean Gaussian errors. 
To correct for this bias, it is included in the filter state space so that it can be continuously 
adjusted as necessary based on other heading measurements. 



2.7 Flux-Gate Magnetic Compass 

The magnetic compass used at the DSL is the C-100, manufactured by KVH 
Industries, Inc. It consists of a toroidal flux-gate sensing element with an associated 
electronics board. The sensor element is a saturable ring core, which floats in an inert fluid 
so the sensing element remains horizontal. Windings surround the lexan housing of the 
sensor element, electrically driving the coil into saturation. Secondary windings then 
sense pulses generated by the horizontal component of the earth’s magnetic field (KVH, 
1992). 

The compass provides heading information at 10 Hz. The manufacturer’s 
specifications list the accuracy as 0.5°, with a 0.1° resolution. However, there are three 
major obstacles to achieving this accuracy when the compass is mounted on a vehicle. 

First, the magnetic compass is referenced to magnetic north rather than true north. 
Therefore, the difference between true and magnetic north, or variation, must be 
compensated for prior to using the heading information for the filter. This variation is 
dependent on the location of the vehicle. While the average variation in an area can easily 
be obtained from the pertinent navigational chart, the actual compass reading can be 
affected by local concentrations of iron beneath the bottom of the sea. Since the vehicle is 
usually operating within 200 m of the ocean bottom, where local magnetic disturbances 
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can be present, performance of the magnetic compass must be monitored to ensure that 
any magnetic disturbances are recognized. 

The second error is deviation, or the difference between a perfect magnetic 
compass and the compass in use. This is influenced by the construction of the compass as 
well as the presence of ferrous metals on the vehicle in the vicinity of the compass. 

The third source of error is more problematic. Whenever the vehicle’s thrusters are 
used, the resulting magnetic field from the energized motors affects the output of the 
compass, resulting in output instability until the thruster-induced magnetic transients 
decay. For large-area mapping operations, the vehicle operates mostly on long, straight 
legs, so the effects of thruster-induced errors are minimized. Operations requiring more 
frequent maneuvering degrade compass accuracy accordingly. 

The characteristics of the magnetic compass are the opposite of the gyro: it does 
not drift with time but does exhibit significant short-term fluctuations. Because of its lack 
of drift, it can be used as a reference to reset the gyro when required. However, care must 
be taken to ensure that the magnetic compass is settled out when performing the reset. 
Most importantly, the vehicle thrusters should not be in operation during this procedure to 
avoid the subsequent error in compass heading output. As for the gyro, magnetic compass 
bias is included in the state space to provide a continuously updated estimate of its value. 



2.8 Inclinometer 

At DSL, the single-axis Watson inclinometer with angular rate sensor is used to 
provide pitch and roll information. This unit combines accuracy and reliability with small 
size and low power requirements, making it ideal for vehicle operations. 

The inclinometer uses a rate sensor and an integral vertical reference. A position 
output is obtained by integrating the angular rate output. By comparing this signal with the 
vertical reference, an error signal is generated. This error signal is filtered and sent back to 
the rate sensor as a bias. The system is also damped. With the overall system, the effects of 
inertia, damping, and short-term accelerations are reduced. 

The angular rate sensor uses two piezoelectric bender elements, which are 
resonantly driven in opposite directions. Rotation causes a bending force, which is 
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Fig. 2-7. Accelerometer sensor exploded view ( Cady , 1984). 



demodulated to produce the rotation rate. The vertical reference uses a liquid-capacitive 
element. With these components, the system has no moving parts, which increases 
reliability. Overall, the system weighs less than eight ounces, making it easy to mount on a 
ROV (Watson, 1990). To obtain the necessary data, two of these units are used on the 
vehicle, mounted orthogonally. One provides pitch, the other roll. 



2.9 Systron Donner MotionPak (Inertial Motion Unit) 

The Systron Donner MotionPak provides the final sensor measurements required 
for the Kalman filter. It consists of two pans: the linear accelerometers and the angular 
velocimeters. 

2.9.1 Accelerometer Operation 

The linear accelerometer used for vehicle navigation in this application is the 
Sundstrand Data Control Q-Flex® accelerometer (see Fig. 2-7). Acceleration produces a 
torque on the sensor’s proof mass. A detector measures the displacement of the mass and 
produces a proportional output voltage. The resultant signal is amplified and fed to a 
torquer coil fixed to the proof mass. This current in the coil provides a restoring torque to 
balance the applied acceleration. The current also goes through a load resistor, generating 
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Fig. 2-8. Accelerometer system functional diagram (Cady, 1984). 



the output voltage for the detector (Cady, 1984). See Fig. 2-8 for an overall system 
schematic. 

2.9.2 Angular Velocity Measurements 

Angular measurements are provided within the IMU using the Systron Donner 
GyroChip™. This device uses a vibrating quartz tuning fork to sense angular rate by 
acting as a Coriolis sensor. The vibrating fork drives a similar pickup fork that produces 
the output signal. These two forks, along with their support flexures and frame, are made 
from a wafer of single-crystal piezoelectric quartz. The drive tines are driven by an 
oscillator that causes the tines to move toward and away from each other at high 
frequency. Each tine has a coriolis force acting on it given by 



F = 2mw x V r , 



where 

m is the tine mass, 

0) is the input rate, 

V y is the instantaneous linear radial velocity. 

The forces generated are perpendicular to the plane of the fork assembly at each of 
the tines and in opposite directions, yielding a torque proportional to the input angular 
velocity. The pickup tines respond to the oscillating torque by moving in and out of the 
plane, causing output signals to be produced by the pickup amplifier (Systron Donner, 
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1993). A separate unit is provided for each axis, resulting in independent measurements 
for pitch rate, roll rate, and yaw rate. 
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Chapter 3 

Development Of The Kalman Filter 
Model 

3.1 Characteristics of the Linear Kalman Filter 

The purpose of the Kalman filter is to produce an unbiased, minimum variance, 
consistent estimate of a quantity* based on a set of measurements, z, where the 
relationship between these variables can be represented by 

z = Hx + v . 

Therefore , z is a linear combination of the elements of the vector .r, plus random noise 
represented asv ( Gelb , 1974). The desired properties of such an estimate, as delineated 
above, are: 

Unbiased: the expected value of the estimate is the same as that of the actual quantity 
being estimated. 

Minimum Variance: the error distribution of the estimate is less than or equal to that of 
any other unbiased estimator. 

Consistent: as the number of measurements increases, the estimate converges to the 
true value of the quantity being estimated. 

If the noise is Gaussian and the relationship is linear, the Kalman filter has been proved to 
meet these criteria (Gelb, 1974). 

An additional characteristic of the Kalman Filter is that it is recursive, which 
means that new estimates can be computed without storing past measurements. Instead, all 
previous information is summarized in the current estimate and associated error 
covariance matrix. This recursiveness allows real-time processing of data without 
excessive memory storage requirements or computational burden. 
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3.1.1 System Model 



For a given real system to be represented using a Kalman filter, it must be put into 
a standard form. The two governing equations of the system model are: 

x — Ax + Gw + Lit 



and 



z = Cx + Du + v , 

where// is a deterministic control input, w is a random forcing function with error 
covariance Q , and v is the measurement noise with error covariance matrix R .A,G, L, C, 
and D are matrices describing the relationship between the different vectors. 

The first equation states that the rate of change of the system parameters can be 
predicted using a linear combination of the present parameters and a linear combination of 
control inputs. The second states that the measurement vector, z , is a linear combination 
of the system parameters plus a linear combination of the present control inputs. Both 
equations include the effects of noise by virtue of then’ andy terms. 

3.1.2 System Model Dynamics 

To produce an appropriate Kalman filter model, the state variables that make up 
the vectors* and- must be defined. The vector.y includes all parameters necessary to 
model vehicle behavior. The vectorr includes all measurements that can be obtained from 
the sensors as described in Chapter 2. Both must be determined using knowledge of the 
system dynamics and the available sensors. In this case, there are two different types of 
model propagation, one for vehicle motion and one tor attitude. 

The primary control forces are those induced on the vehicle by the tow cable. 
However, these are virtually impossible to predict due to the realities of the towing 
operation. First, although the surface ship is proceeding predominantly on a known 
course, wave action produces continuous variations in pitch, roll, and heading. These 
variations are all propagated down the tow line to some degree, despite various techniques 
used to isolate vehicle motion from ship motion. Even if these forces on the tow cable at 
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Fig. 3-1. System model for X-position propagation. 



the surface could be measured precisely, the cable forces at the vehicle, several thousand 
meters away, are difficult to model. 

The vehicle model is also ill-defined. Changes in equipment configuration on the 
vehicle, which affect drag and added mass, can have significant effects on the vehicle 
dynamics. Also, when cable forces induce a change in vehicle attitude, the force of the 
water acting on the moving vehicle coupled with the designed self-righting characteristics 
of the vehicle combine to restore the vehicle to its mean velocity and attitude. Thus, the 
vehicle frequently follows a damped sinusoidal path in both vertical and horizontal 
directions. Therefore, the predominant characteristics of vehicle motion are: 

1. It is extremely unpredictable due to the inability to measure cable forces and to 
properly model vehicle dynamics. 

2. The vehicle tends to return to its mean attitude after a cable disturbance has sub- 
sided. 

As a result, the vehicle accelerations are be considered to be influenced by a white- 
noise disturbance and control inputs are not modelled. To help compensate for any errors 
in these assumptions, linear accelerations are measured by the strapdown inertial motion 
unit (IMU) to provide current, though noisy, actual accelerations to the filter algorithm. 

For thruster-controlled vehicles, control input is often better defined. In these 
circumstances, operator-controlled inputs can be added as a deterministic vector w, with 
an associated matrix L to mathematically relate the inputs to their effect on the state 
vector. 

To estimate vehicle position, the system model is as shown in Figure 3-1. X- 
position (latitude) is shown; Y-position (longitude) and Z-position (depth) follow the same 
principles. 
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Fig. 3-2. System model for vehicle propagation. 



The system model for the angular positions is similar but requires only one 
integrator. Again, the system is modelled as disturbed by white noise. The angular 
velocity output of the IMU is used as the measurement for angular velocities (roll, pitch, 
and heading), shown in Figure 3-2. 



3.2 Sensor Outputs and Their Relationship to the 
State Space 

Before proceeding to the specifics of the state space model for this application, 
some details of the sensor data output must be described. To produce a useful Kalman 
filter for this application, it is essential to identify exactly what information is available. 
Additionally, an expected frequency for receipt of data by the filter is necessary. 

During vehicle operations conducted by DSL, four different data packages are 
collected as the appropriate data are available. These four data streams must be translated 
into a format usable by the Kalman filter, and any potential gaps in receiving data must be 
anticipated and accounted for in the filter algorithm. 

3.2.1 IMU Package 

The IMU package consists of the linear accelerometers and angular velocimeters. 
As is the case for the other three data packages, the raw data stream is translated into a 
usable form for t,he filter using a relatively simple program written in C language. The 
IMU package provides data at a rate of approximately 10 Hz. The IMU’s outputs are all in 
sensor coordinates, which must be converted to body coordinates for use in the filter. 
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3.2.2 Attitude Package 



The attitude package combines the outputs of the depth detector, inclinometers, 
gyro, and compass to provide depth, roll, pitch, and heading. While individual sensors 
providing data operate at various frequencies, the overall package operates at 
approximately 5 Hz. 

3.2.3 Doppler Velocimeter Package 

Included in each data stream from the doppler velocimeter is considerable detail 
about the operator-selected parameters being used by the velocimeter. However, only the 
body-referenced velocities in each orthogonal direction and the velocity error 
measurement are used by the Kalman filter. Again, a C program is used to translate the 
Hex-ASCII output into usable data, and the sensor-referenced outputs must be converted 
to body coordinates. 

As discussed in Chapter 2, the output frequency of the doppler velocimeter 
depends primarily on the number of pings per ensemble and the altitude of the vehicle 
above the bottom. For the sidescan-sonar mapping configuration employed during the 
DSL- 120 deployment used for this thesis, the doppler velocimeter usually provided data 
approximately every 2-3 s. However, this time period was occasionally shortened or 
lengthened due to changing altitude and varying bottom conditions. 

3.2.4 Positioning System Package 

The positioning system has the most irregular of the four data streams received for 
vehicle navigation. Frequency of data reception depends on the type of positioning system 
in use. For systems using an acoustic transponder array, such as the long-baseline or short- 
baseline systems, this depends primarily on the distance from the vehicle to the acoustic 
transponder array. However, whenever data acquisition is secured (usually when a given 
track line is completed until the towing ship has maneuvered to get into position for the 
next track line), the positioning system is frequently secured as well, since precise 
knowledge of vehicle location is unnecessary except when obtaining sonar data. Of more 
concern is a loss of position information during sonar operations. There are several 
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different possibilities that can cause such a problem. A large bottom feature, such as a 
ridge, can physically block the path from the acoustic array to the vehicle. Another 
significant factor is the sound-velocity profile (SVP), which dictates the path sound waves 
will travel in the water. With an unfavorable SVP, acoustic position information can be 
lost only a short distance away from the transponder array. 

For systems not tied to an array, such as a terrain-relative positioning system, the 
frequency of obtaining information will be highly unpredictable since it depends on the 
available terrain and the quality of the database. It is likely that there will be frequent long 
gaps between fixes and that these gaps will exist whether or not sonar operations are in 
progress. 

This irregularity of position information is an important reason for developing a 
Kalman filter using the doppler velocimeter information. By filling in the gaps in position 
fixes with an accurate estimate of vehicle position, the sonar data quality can be greatly 
enhanced. 

Like the attitude data stream, the position data stream is typically easy to decipher. 
The components used for this Kalman filter are the X and Y positions (in meters from a 
reference point) and the time the datum was obtained. 



3.3 Defining the State Space 

Now that the system dynamics and available measurements are clearly identified, 
the state space can be defined. Once the.y vector is labeled, the Kalman filter model can be 
mathematically developed. Using the coordinate system axes specified in Section 2.2, the 
components of the state vector.y are defined as: 

XI: X-acceleration (All accelerations are in body, or vehicle-referenced, coordinates) 
X2: Y-acceleration 
X3: Z-acceleration 

X4: X-velocity (All velocities are in body, or vehicle-referenced, coordinates) 

X5: Y-velocity 
X6: Z-velocity 
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X7: X-position (All positions are in earth-referenced coordinates) 

X8: Y-position 
X9: Z-position 

X10: Roll rate (Defined as rotation about the vehicle’s Y-axis; positive roll is defined 
as port side up) 

XI 1 : Pitch rate (Defined as rotation about the vehicle’s X-axis; positive pitch is 
defined as bow up) 

X12: Yaw rate (Defined as rotation about the vehicle’s Z-axis; heading is measured 
with normal compass orientation. To maintain the right-handedness of the system, this 
is converted within the Kalman filter algorithm to measure positive heading rate, 
usually called yaw rate, as counterclockwise, rather than the normal compass direction 
of clockwise) 

X13; Roll 

X14: Pitch 

X15; Heading 

X16: Gyro bias (Difference between gyro heading and true heading) 

X17; Magnetic compass bias (Difference between compass heading and true heading. 
It includes both variation and deviation) 

Also, there is the measurement vector, r . When all four data streams are available to the 
filter, the components ofz correspond exactly to the components of the state vector .r , 
with the addition of two heading measurements from the gyro and compass. As discussed 
later, however, this is seldom the case due to the variations in data stream frequency. 



3.4 Nonlinearity and the Extended Kalman Filter 

Unfortunately, the differences in coordinate systems between the various sensors 
means that the model cannot be treated as linear. With vehicle accelerations and velocities 
provided in body coordinates and positions provided in earth-referenced coordinates, a 
coordinate transformation must be made between the two references to allow the data to 
be used. This coordinate transformation is accomplished using standard trigonometric 
equations that use vehicle roll, pitch, and heading to relate body coordinates to earth 
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coordinates. This transformation is performed within the extended Kalman filter algorithm 
by altering the A matrix at each filter iteration as explained in Section 3. 7. 2. 2 

3.4.1 Choosing the Extended Kalman Filter 

As stated in Section 3.1, the linear Kalman filter has been proved to provide an 
unbiased, minimum-variance, consistent estimate of the state vector.v as long as the noise 
is Gaussian. Noise considerations are addressed in Section 3.5. 

To compensate for the nonlinearity of the system, several techniques are available. 
Unfortunately, none of these can be proved to meet the same standards as the linear 
Kalman filter. In order of complexity, three possible solutions are the extended Kalman 
filter, the iterated extended Kalman filter, and the second-order filter ( Gelb , 1974). 
Because it is the simplest and incurs the least computational burden, the extended Kalman 
filter is used to provide the state estimates for this application. As Gelb states. 

There is no guarantee that the actual estimate obtained will be close to the truly 
optimal estimate. Fortunately, the extended Kalman filter has been found to yield accurate 
estimates in a number of important practical applications (Gelb, 1974). 

3.4.2 Filter Propagation 

The basic method of producing a state estimate with the extended Kalman filter is 
the same as for the linear version. First, some definitions must be provided: 

x (rp) is the estimate of the state vector using all data up to and including the present 
time. 

x (t + 1 p) is the predicted estimate of the state vector at the time of the next filter 
update step using all data up to and including the present time. 

x (r|r - 1) is the predicted estimate of the state vector at the present time using data 
up to and including the time of the last filter step. It is the same as the.v (t + 1 |r) 
calculated after the previous filter update step. 

P (r|r) is the error covariance matrix of the state vector using all data up to and 
including the present time. 

P (t + 1 |r) is the predicted covariance matrix of the state vector at the time of the next 
filter update step. 

P (r| t - 1 ) is the predicted estimate of the error covariance matrix at the present time 
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using data up to and including the time of the last filter step. It is the same as 
th cP (t+ 1 \t) calculated after the previous filter update step. 

For the linear Kalman filter, the error covariance matrices are deterministic and 
exact based on the assumptions used for input and measurement noise in the filter model. 
Therefore, they provide the current uncertainty of each variable in the state vector, 
assuming the model is correct. For the extended Kalman filter, the error covariance is only 
an approximadon since the actual error covariance matrix cannot be calculated due to the 
nonlinearity of the system, a condition that can theoretically cause filter divergence. 
However, the extended Kalman filter has been found to work in numerous practical 
applications, with the only test being whether the filter works in actual use ( Gelb , 1974). 
As demonstrated in Chapter 4, the good results using simulated and actual data support the 
assumption that the extended Kalman filter is an appropriate choice for this application. 

For filter propagation, current estimates of.v (f|/- 1) andP (t\t— 1) are available 
from the previous filter iteration. As explained in Section 3.1.1, the governing equations 
for the system model are: 



■i = Ax + Gw 



and 



z — C.v + Du . 

TheD» term is called the feed-forward term and represents the immediate effect of 
control inputs on output. As discussed earlier, control inputs are not modeled. 

3.4.2. 1 Update Step 

The first step in the filter propagation is to determine the innovation term /, which 
is defined by the equation: 

i = (Cx.x(t\t- 1 )) . 

This is the difference between the expected measurement vector obtained from the state 
estimate .r {t + 1 \t) calculated during the previous filter update step and the actual 



42 



measurement vector derived from the vehicle’s sensors. Next, the covariance matrix of 
innovation, v, is calculated: 



v = CxP(t\t- 1) xC' + R, 

where/? is the error covariance matrix of the measurement vector, z . The Kalman gain A* 
is then calculated: 



K = P (t\t- 1) x C' x inv (v) . 

The next step is to calculate the new state vector estimate using the value of.y (/ + 1 1 /) 
determined during the previous filter update step: 

x(t\t) = - v ( / 1 r — 1 ) +KxI. 

Finally, the new error covariance is computed: 

P(t\t) = (Ident-KxC) xP(t\t- 1) x {Ident - K xC)' + KxRxtC , 

where Ident in this case is the identity matrix with the same dimensions as KxC . 

3A.2.2 Prediction Step 

For the standard Kalman filter, the predicted values of the state vector and the error 
covariance matrix for the next time step using data up to the present time are now 
calculated. Since the Kalman filter is implemented as a discrete instead of continuous 
filter, a conversion of the propagation equations from their continuous forms given in 
Section 3.1.1 to their discrete counterparts is performed. This transformation uses the 
MATLAB function “c2d”, which takes the A and G matrices and the filter time step as 
inputs and provides the discrete matrices^ (translation of the A matrix) andT (translation 
of the G matrix) as outputs. Once the conversion to discrete matrices is made, the 
predictions are computed: 

.v (/ + 1 1 0 = <(> x x (r| t) + f x w , 
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p(r+i|0 = <t> x p (f|o x + r x q x r' , 



where Q is the error covariance matrix for the while noise, vv . 

3.4.3 Implications of the Extended Kalman Filter 

To obtain a new state estimate, the extended Kalman filter uses a first-order Taylor 
series expansion about the current state estimate to linearize the problem at each update 
step. In the steady-state version of a linear Kalman filter, the Kalman gain K is a constant. 
For the extended Kalman filter, it must be recomputed at each update step ( Gelb , 1974). 



3.5 Modeling System Noise 

Prior to using the Kalman filter, predictions must be made concerning the 
estimated errors in the sensors used. All data used by the filter are weighted based on these 
predicted errors. A measurement considered very accurate has a strong effect on the state 
estimate. For example, if position information is considered very accurate relative to 
heading and velocity, the estimates of the latter two are altered from their measured values 
as necessary to ensure that the estimate of position is close to the measured value. Poor 
position information places more reliance on heading and velocity measurements. To 
ensure optimum filter performance, it is essential that the errors be modelled as aecurately 
as possible. 

Another important assumption in the Kalman filter is that any errors in the control 
input vector and the measurement vector are unbiased and follow a Gaussian distribution. 
For nearly all cases, the assumption of Gaussian errors is valid considering the 
characteristics of the sensors employed. The prominent exception is the positioning 
system. While normally the errors can be treated as Gaussian, occasionally the system 
produces anomalously large errors in position. If these position fixes are treated by the 
filter as valid, improper changes in the elements of the state vector are made, resulting in 
rapid filter degradation. Identifying and ignoring these bad data is an integral part of the 
modified filter algorithm. 
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3.5.1 Eliminating Non-Gaussian Position System Errors 

The grossly inaccurate position measurements occasionally produced by acoustic 
positioning systems are frequently referred to as “flyers”. Removing them from the 
database is vital, but care must be taken. If the allowable tolerance between estimated 
position and measured position is too tight, valid position fixes can be rejected by the 
filter. Once this divergence between estimated and actual position occurs, the likelihood of 
ever accepting another position fix is greatly reduced. This problem is most acute when a 
long interval exists between fixes. If vehicle heading or velocity is estimated inaccurately 
during this interval, the difference between estimated and actual vehicle position grows. If 
the discrepancy becomes large enough, when position information is once again obtained 
it is considered to be a flyer and rejected. With no further position information, the state 
estimate of position becomes increasingly inaccurate. 

To provide a window that is likely to reject flyers while accepting all valid fixes, 
the standard allowable error is established at ten times the standard deviation of the 
position system. To provide for time-dependent system errors, an addition is made to the 
allowable error based on the time since the last position system fix, given by 

Additional allowable error = 0. 1 m x At , 



where 

At = Time since last position fix in seconds. 

Fixes that yield a difference between the current estimated position and the position 
system fix of greater than this total allowable error are rejected. The method tor 
determining the standard deviation is discussed in Section 3. 5. 3. 2. 

The decision to use a cutoff point of ten times the standard deviation is not based 
on any formula. It is large enough to admit reasonable data, but still should reject any 
flyers. To help prevent filter divergence between estimated position and valid fix 
information, a count is maintained within the program of the number of rejected fixes. By 
monitoring this count during vehicle operations, filter divergence can be recognized by 
continuous fix rejection. The window of acceptance can then be adjusted as necessary to 
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allow the filter to aeeept new fixes and thus ehange the state estimates as appropriate to 
reflect the valid position information. 

3.5.2 Random Forcing Function Noise 

The random forcing function noise is expressed by the error covariance matrix Q . 
It is a 6x6 matrix since there are six terms that drive the system, three linear accelerations 
and three angular velocities. It is all zeros except on the main diagonal since each of these 
parameters is considered to be independent of the others, making the cross correlation 
terms zero. 

The magnitude of the error covariance terms must be estimated from knowledge of 
the vehicle dynamics accounting for tow ship motion, tow line motion, and vehicle 
dynamics. For this filter, the standard deviation of the linear accelerations is set at 0.2 m/ 
s , and that of the angular velocimeters at 2 °/s. These constants are incorporated into the 
KFSetup program. 

3.5.3 Measurement Uncertainty 

The measurement uncertainty is incorporated into the 17x17 error covariance 
matrix R . The portions of this matrix applicable to each sensor are addressed separately 
below. 

3.5.3.1 Linear Accelerometer Uncertainty 

The uncertainties in the linear accelerometer measurements have two sources. One 
is the noise and bias inherent in any instrument, which can be determined by test or by 
using the manufacturer’s specifications. The other is caused by the displacement of the 
sensor from the vehicle’s reference point for the body- referenced coordinate system. 

As shown in Figure 3-3, the vehicle’s center of rotation and therefore its most 
convenient body-coordinate reference point is the point of attachment to the tow line. 
Assuming the vehicle is not experiencing an actual linear acceleration to the left (-X 
direction), the X-aecelerometer would still sense acceleration if angular velocity in the 
yaw direction (change in heading) were non-zero. This phenomenon is governed'by the 
following coupled equations ( Catfonl , 1978): 
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Fig. 3-3. Linear accelerometer output caused by angular velocity. 



= u + qw-vr + a x (-cj2-r2) + r/y (- r + pq) + a, (q + pr) + gsin0 



a y = v - pw + ur + a x (r + qp) + a y (-/?2 _ r~) +a~(—p + qr) -£sin<|)eos0 



= w + pv - qn + a x (- q + rp) + a y (p + rq) + a z {- p~ - r/2) - g cos 0 cost}) 



where , ay , anda.. are the outputs of the accelerometers; a x , r/ v , and a : are actual 
linear accelerations; //, v, and w are velocities; <()= roll; 0 = pitch; p, q, r are the angular 
velocities; g is the gravity vector. 

To simplify the filter algorithm, this addition to linear acceleration measurements 
due to angular velocities is ignored. The justification is twofold: first, for a towed vehicle 
the magnitude of the linear acceleration caused by the angular velocities is small 
compared to actual linear accelerations; second, over a relatively small time interval, the 
angular velocities have a mean of zero due to the damped sinusoidal motion of the vehicle. 
Therefore, any errors induced by this simplification are cancelled out when the angular 
velocity reverses direction. As a result, the standard deviation for the linear acceleration 
measurements are set at 0.1 m/s . 

3.S.3.2 Doppler Velocimeter Uncertainty 

As part of its data stream, the doppler velocimeter provides an error velocity. This 
error velocity is calculated by the velocimeter using a proprietary RDI Instruments 
program. Factored into this calculation are the depth cell size, the altitude of the vehicle 
above the bottom, the vehicle velocity, and the number of pings per ensemble (RDI, 1993). 
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This error velocity is used by the Kalman filter program as the standard deviation for 
velocity measurements. Therefore, these elements of the/? matrix vary with each filter 
update step. 

3.5.3.3 Position System Uncertainty 

The standard deviation for acoustic positioning systems is determined based on 
operator knowledge of the system and takes into account such parameters as system 
frequency and distance from the transponder array. It is established prior to filter operation 
as part of the KFSetup program but can be changed during operations if desired. For other 
systems, such as terrain-relative position fixes, the standard deviation can he made part of 
the data stream based on the estimated accuracy of the fix and can therefore be varied from 
fix to fix. 

Of all the measurement uncertainties, this requires the greatest attention. If it is 
assigned to be too large, the Kalman filter does not weight position fixes enough, resulting 
in potentially significant errors in the estimated position. This can adversely affect the 
quality of any sidescan-sonar or video data obtained during vehicle operations. 
Conversely, if it is set too small there is an increased danger of creating sufficient 
divergence between the estimated and actual positions to cause invalid position fix 
rejections. 

3.5.3.4 Depth Sensor Uncertainty 

When operating in deep water, the depth sensor has one important drawback: 
because of the limitations of its computer bit capacity, it can only resolve depth to the 
nearest 0. 1m. Coupled with the normal fluctuations of the instrument, this tends to make 
the output vary frequently within a 0.4-m band. Therefore, the standard deviation for 
deep-water operations is set at 0.1m, which places nearly all fluctuations within two 
standard deviations of the actual value. This standard deviation can be reduced 
appropriately for shallow-water operations when the resolution is higher. 
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3.5.3.5 Angular Velocimeter Uncertainty 

Estimation of the angular velocimeter uncertainty is fairly straightforward. Prior 
testing or manufacturer’s specifications are used to determine the expected standard 
deviation. For the instruments in use, this is set at 0.5°/s. 

3.5.3.6 Pitch and Roll Uncertainty 

Like the depth sensor, the pitch and roll sensors are digital and therefore have a 
noticeable minimum resolution. For these sensors this resolution is 0.1°, and normal 
fluctuations in a 0.2° band are observed. To account for unmeasurable biases and to 
prevent the filter from weighting these measurements too heavily, a standard deviation of 
0.5° is assigned. 

3.5.3.7 Heading Uncertainty 

Heading is measured directly by the magnetic compass and the gyro. For each, the 
applicable formula is 

True Heading = Indicated Heading - Sensor Bias . 

Since the bias for each sensor is modeled within the Kalman filter as a state variable, 
additional measurements improve the estimate of bias and thereby make the estimate of 
true heading more accurate. 

The standard deviation of the magnetic compass is constant and is set at 0.5° to 
account for normal fluctuations. If frequent thruster operations are envisioned, this 
standard deviation may have to be increased due to the effects of the magnetic fields 
induced during thruster operations. Due to its greater stability, the gyro has a constant 
standard deviation set at 0.1°. 

To provide an independent measurement of true heading, which is necessary to 
correct sensor biases, two different techniques are possible. For a highly maneuverable 
vehicle operating in a small area, an acoustic transponder can be mounted both at the front 
and at the back of the vehicle. By comparing the positions of each transponder, true 
heading can be derived. The standard deviation for this technique depends on the accuracy 
of the positioning system used. 
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Fig. 3-4. Calculation of true heading and standard deviation. 



For a towed vehicle, vehicle motion can be assumed to be nearly entirely in the 
direction of the vehicle’s front. Therefore, a measurement of true heading is obtained by 
calculating a least-squares lit to the last four position fixes. Although the standard 
deviation of this heading measurement is difficult to calculate precisely, an approximation 
is derived as follows (see Fig. 3-4). 

After obtaining the least-squares solution, a separate heading measurement is 
made between each adjacent pair of fixes (l and 2, 2 and 3, 3 and 4). The difference 
between each of these heading measurements and the overall least-squares measurement 
is averaged. This average difference in heading measurements is considered to be the 
standard deviation of the measurement. Although this is not precise, it has the desired 
property of providing less standard deviation as the fix measurements become less 
scattered from the best-fit track. 



3.6 Establishing Reference Points and Initial Conditions 

3.6.1Reference Points 

To ensure correct matrix manipulations while avoiding round-off errors, MATLAB 
works better with well-conditioned matrices, which means that the elements of the 
matrices are as close together as possible in terms of order of magnitude. To achieve this 
conditioning, reference points for position and time must be established. These reference 
values are subtracted from the actual values prior to use in the filter. The filter uses only 
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the difference between the reference values and the current values, resulting in numbers 
smaller by several orders of magnitude. 

For X- and Y-positions, the Universal Transverse Mercator (UTM) system is used. 
This system divides the oceans into rectangular grids with scales in meters. A suitable 
reference point is chosen in the area of vehicle operations and included in the KFSetup 
program. 

The Z-position reference point is dependent on expected vehicle operating depth. 
To achieve a right-handed coordinate system, the filter actually uses vehicle altitude above 
or below the reference point rather than depth, but this coordinate system change is 
imbedded within the filter algorithm. A reference depth near the expected operating depth 
is chosen by the operator in the KFSetup program prior to commencing operations. 

The final reference point is time. Each sensor package data stream contains a time 
stamp with units of Julian days. The reference time for each day is the beginning of the 
Julian day. Therefore, the time value used in the filter is always a fraction of a day. 

3.6.2 Initial State Vector and Error Covariance Matrix 

Prior to the first filter update step, the initial state vector* and error covariance 
matrixP must be provided. Normally, the initial sensor data obtained are used for the state 
vector, and the predetermined/? matrix is augmented with the initial calculated variable 
elements of R (for velocimeter and heading measurements) to provide P. However, if 
some other more accurate method is available for determining any of these variables, the 
better value can be substituted instead. 

3.6.3 Removing Known Biases 

Of crucial importance in optimizing Kalman filter performance is the removal of 
bias. Uncorrected bias provides a consistent source of error in the state estimate vector and 
must be eliminated as much as possible. 

Because of its use of doppler to measure velocity, whereby zero frequency change 
equates to zero velocity in that direction, the doppler velocimeter is relatively immune to 
bias. Therefore, no attempt is made to compensate for it. 
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The position information is also insensitive to bias, since the use of a reference 
point automatically removes bias from the system. Once again, no bias correction is 
needed. 

The bias of the pitch and roll sensors is directly related to the precision of their 
placement on the vehicle. Normally, the final installation of the sensors on the vehicle is 
performed in the lab, which allows the sensors to be checked to ensure they are unbiased. 
Therefore, no bias adjustment is made in the filter. If the sensors must be replaced at sea, 
accurate measurement of pitch and roll on a moving ship is virtually impossible, so there 
is no way to measure bias accurately. In this ease, the sensor is mounted as precisely as 
possible and the filter assumes unbiased measurements. Since pitch and roll are usually 
rather small, the effects of undetermined bias on the coordinate transformations is 
negligible. 

Compensation for gyro bias and magnetic compass bias is discussed in Section 
3. 5. 3. 5. Additionally, any known initial bias is included as the initial condition of the state 
variable for each heading sensor bias. Short-term effects on magnetic compass bias caused 
by thruster operation cannot be calculated and are therefore ignored. 

As discussed in Section 3.5.2, the biases inherent to the linear accelerometers and 
angular velocimeters must be determined by laboratory testing. With the vehicle 
motionless and with zero pitch and roll, the output voltage of each instrument can be 
measured. The voltage caused by the gravity vector is calculated so as not to be removed 
from the instrument measuring vertical acceleration, but the other measured voltages are 
subtracted from the data. These biases are also included in the KFSetup program. 



3.7 Changes Required to the Standard Kalman Filter 

In this application, there are three major changes that must be made to the standard 
Kalman filter described in Section 3.4. First, due to the variability of the data update rate, 
a constant filter update frequency is impractical. Second, the difference in coordinate 
systems among the various sensors must be addressed. Finally, there are gaps in certain 
data streams, especially for X- and Y-position fixes. The filter must be able to function 
despite these gaps to provide continuous state estimates. 



52 



3.7.1 Adjustments Required Due to Varying Filter Update Rate 

3.7.1. 1 Determining the Desired Time Step 

To determine the desired time step, eompeting objectives must be considered. In 
support of a longer time step, there are two major arguments. First, adequate time must be 
given to obtain the data necessary for the propagation of the filter. Second, the interval 
must be long enough to permit real-time processing of the data. 

There is only one counterpoint in support of a shorter time step, but it is a vital one. 
If the time step is too long, the true vehicle dynamics will be lost, especially if significant 
maneuvering is occurring. Although a long time step may provide a reasonable 
approximation of average vehicle position and attitude, the lack of precise, continuous 
information will degrade any sidescan-sonar or video imaging accuracy. A compromise is 
necessary. 

Of the four data packages described in Section 3.2, only two have update rates that 
are both consistent and rapid. The 1MU package produces data at approximately 10 Hz, 
while the attitude package operates at about 5 Hz. Therefore, the Kalman filter is designed 
with a minimum requirement of having data from these two packages prior to performing 
a filter update step. As an overall compromise, a lime step of 0.5 s is used. This is short 
enough to capture the system dynamics, but long enough to allow real-time processing of 
data and to ensure both IMU and attitude package information is normally available. 

To prevent filter processing error in the event of an interruption in data flow, the 
filter algorithm checks to ensure that at least one sample of each data package is available 
within the 0.5 s of data to be processed. If either is missing, the filter continues to accept 
new data until both are present. As soon as this condition is met, the data obtained from 
each sensor during that time step are averaged and the next filter update step is performed. 

3.7. 1.2 Predicting the Next State Vector and Error Covariance Matrix 

\ 

In the standard Kalman filter, the last step of filter propagation produces the 
predicted values of.r (r + 1 |r) and P (t + 1 |r) .As described in Section 3. 4. 2. 2, one of the 
inputs required for the conversion from a continuous to a discrete matrix is the magnitude 
of the time step. For this filter, that magnitude is unknown. First, it will probably not be 
exactly 0.5 s, since the actual time step is the difference in lime between the first and the 
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last data received prior to 0.5 s elapsing. Second, it' either IMU package or attitude 
package data is not received during that first 0.5-s interval, the filter algorithm waits until 
the missing data are provided. Thus, the time interval could be considerably longer. 

To correct for these varying time steps, a small change in filter mechanics is 
required. Instead of computing the predicted values at the end of a given filter update step, 
the filter algorithm waits until the beginning of the next filter update cycle. Therefore, the 
filter time step is known since the algorithm has already allowed 0.5 s to elapse and has 
then checked for the two data streams, waiting as necessary to obtain them. By allowing 
the predicted values to lag until the next time step is confirmed, the continuous-to-discrete 
conversion can be made accurately. 

3.7.2 Coordinate System Transformations 

Due to the difference in coordinate systems among the various sensors, two 
separate calculations are required. Both use the current estimates of roll, pitch, and 
heading as input. The first removes the effect of the gravity vector on the linear 
accelerometer measurements. The second computes the current/! matrix used in the 
prediction of the next state estimate and error covariance matrix. 

3.7.2. 1 Removing the Effect of the Gravity Vector 

To provide an accurate reflection of vehicle linear acceleration, the output ol the 
IMU package must be modified to remove the gravity vector bias. This vector g is 
assumed constant with a value of 9.8 m/s 2 pointing in the -z direction in earth-referenced 
coordinates. The filter algorithm uses the current state estimates of roll and pitch to 
calculate the component of the gravity vector included in the output of each ot the three 
linear accelerometers. The effect of the gravity vector on x-acceleration is shown in Fig. 
3-5 as an example. The calculations are as follows (<j) = roll, to = pitch, g = -9.8 ): 

% 

Actual x-acceleration = (Measured x-acceleration) +gsin<|). 



Actual y-acceleration = (Measured y-acceleration) -g sin to. 
Actual z-acceleralion - (Measured z-acceleration) - geos <|> cos G3 . 
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Fig. 3-5. Effect of gravity vector on measured x-acceleration. 



These calculations are actually approximations because the effect of one attitude 
on the measurement of another is ignored. For example, as pitch increases the sensitivity 
of the roll sensor is decreased due to the lessened gravity vector component on the 
inclinometer’s sensitive axis. In the worst case, a pitch of 90 degrees results in no 
sensitivity to roll. For typical vehicle operations, during which pitch and roll are normally 
less than 10 degrees, the errors introduced by these approximations are negligible. 

3.7. 2.2 Calculating the A Matrix 

As explained in Section 3. 4.2. 2, part of the prediction step of the filter update cycle 
converts the continuous time A and G matrices into their discrete time counterparts. 
Unlike the linear filter case, in this nonlinear filter the A matrix must be recalculated 
during each prediction step. 

To be more specific, the detailed version of the system propagation equation is as 
follows. The A matrix is constant with the exception of the 3x3 portion identified with 
“a because this portion of the A matrix propagates the change in vehicle position due to 
the previously estimated vehicle velocities. Since vehicle velocities are body- referenced 
and positions are earth-referenced, a coordinate transformation is necessary. 

The current heading estimate, which is maintained in the traditional clockwise- 
positive system used by the compass and gyro, is altered to a counterclockwise-positive 
yaw angle to maintain a right-handed coordinate system. With units of radians, this yaw 
angle is calculated by: 



55 



0 0 0 0 0 0 0 0 0 0 0 () () () () () () 




1 0 0 0 0 0 


0 0 0 0 0 0 0 0 0 0 0 0 () 0 () 0 () 




0 1 0 0 0 0 


0 0 0 0 0 0 0 0 0 0 0 0 () () () () () 




0 0 1 0 0 0 


1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 () () 




0 0 0 0 0 0 


0 1 0 0 0 0 0 0 0 0 0 0 0 0 () () () 




0 0 0 0 0 0 


0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 




0 0 0 0 0 0 


0 0 0 a a a 0 0 0 0 0 0 0 () 0 () () 




0 0 0 0 0 0 


0 0 0 a a a 0 0 0 0 0 0 0 0 0 0 0 
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0 0 0 0 0 0 
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A Matrix 
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G Matrix 



k = 2n - (Current Heading Estimate) . 



The 3x3 nonzero matrix within the A matrix is computed as follows. Required inputs are 
■the current state estimates of roll ((()), pitch (to), and heading (k). 

Element (1,1)= cos (() cos k 
Element (1,2) = sincosinejjeosK- coscosinK 
Element (1,3) = cos cosine)) cos k + sincosinic 
Element (2,1) = sinKcoscj) 

Element (2,2) = sin cosin <)> sin k + cos cocos k 
Element (2,3) = cosG)sinc|>sinK- sin cocos k 
Element (3,1) = -sinc|) 

Element (3,2) = sincocoscj) 

Element (3,3) = coscocoscj) 
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3.7.3 Varying the C Matrix with the Available Data 

Section 3.7. 1.1 de.scrihe.s the process by which the filter ensures it has both IMU 
and attitude information available prior to performing an update step. During this period 
of accepting new data, doppler velocimeter and/or X-Y position information may also be 
obtained. To perform the update step correctly in light of the available data, the filter 
algorithm keeps track of which data are available prior to performing the update operation. 
After averaging all data from each sensor, the C matrix must be computed for use by the 
filter. As part of this computation, the measurement vectors is also determined based on 
which sets of data have been obtained. 

After the first four position fixes have been obtained (and therefore true heading 
information is available with each subsequent fix), there are four possibilities for the C 
matrix and measurement vector: 

1. Only IMU package and attitude package data are available. The measurement vec- 
tor is: 



x-accelcration 
y-acceleration 
z-acceleration 
z-position 
roll rate 
pitch rate 
yaw rate 
roll 
pitch 

heading by gyro 
heading by compass 



and the corresponding C matrix is: 
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-1 



reflecting the position in the state estimate vector* of x-acceleration (xl). y-accel- 
eration (x2), z-acceleration (x.3), z-position (x9), roll rate (xlO), pitch rate (xl 1), 
yaw rate (xl2), roll (xl.3), pitch (xl4), heading (xl5), gyro bias (xl6), and com- 
pass bias (x 17). 

2. Both attitude package and XY position information are available but no doppler 
velocimeter data. The measurement vector is: 



x-acceleration 
y-aeceleration 
z-acceleration 
x-position 
y- position 
z-position 
roll rate 
pitch rate 
yaw rate 
roll 
pitch 

heading by gyro 
heading by compass 
true heading 



and the corresponding C matrix is: 
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c = 



1 0 0 0 0 0 0 0 0 0 0 0 0 () () () () 

0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 

0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 

0 0 0 0 0 0 1 0 0 0 0 0 0 {) 0 () 0 

0 0 0 0 0 0 0 1 0 0 0 0 0 () 0 0 0 

0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 () 0 

0 0 0 0 0 0 0 0 0 1 0 0 0 () 0 () 0 

0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 

0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 

0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 

0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 

0 0 0 0 0 0 0 0 0 0 0 ()()() I -1 0 
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 -1 

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 



3. Both attitude package and doppler velocimeter data are available but no XY posi- 
tion information. The measurement vector is: 



x-acceleration 
y-acceleration 
'/-acceleration 
x-velocity 
y-velocity 
/-velocity 
/-position 
roll rate 
pitch rate 
yaw rate 
roll 
pitch 

heading by gyro 
heading by compass 



and the corresponding C matrix is: 



c = 



1 0 0 0 0 0 0 0 0 0 () () () () () () 0 

0 1 0 0 0 0 0 0 0 () () () () 0 () 0 0 

0 0 1 0 0 0 0 0 0 () 0 0 0 0 () () 0 

0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 

0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 

0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 

0 0 0 0 0 0 0 0 1 0 0 0 0 0 () 0 () 

0 0 0 0 0 0 0 0 0 l 0 0 0 0 () 0 0 

0 0 0 0 0 0 0 0 0 0 1 0 0 0 () () () 

0 0 0 0 0 0 0 0 0 () 0 1 0 () () 0 0 

0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 

0 0 0 0 0 0 0 0 0 0 0 0 0 1 () 0 0 

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 0 

0 0 0 0 0 0 0 0 0 0 0 0 0 0 [ 0 - 1 ^ 



4. The final case is when data are available from all sensors. The measurement vector 
uses the same construction as the state vector. 



x-acceleralion 
y-acceleration 
z-acceleration 
x-velocity 
y-velocity 
z-velocity 
x-position 
y-position 
z-posilion 
roll rate 
pitch rale 
yaw rale 
roll 
pitch 

heading by gyro 
heading by compass 
true heading 



while the C matrix is: 
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c = 



1 0 0 0 0 0 0 0 0 0 0 0 0 () () () () 

0 1 0 0 0 0 0 0 0 0 0 0 0 0 () 0 () 

0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 

0 0 0 1 0 0 0 0 0 0 0 0 0 0 () () () 

0 0 0 0 1 0 0 0 0 0 0 0 0 () () () () 

0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 

0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 

0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 

0 0 0 0 0 0 0 0 1 0 0 0 0 0 () () () 

0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 () 0 

0 0 0 0 0 0 0 0 0 0 1 0 0 () () () () 

0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 

0 0 0 0 0 0 0 0 0 0 0 0 1 0 () 0 () 

0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 0 

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 -1 

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 



By using this varying C matrix, the Kalman filter mechanics remain the same for 
each update step regardless of the available data. Additionally, the error covariance matrix 
automatically reflects the data available. For each filter update step, each state variable’s 
error covariance is increased due to elapsed time, and then decreased if a measurement of 
that state variable is available. If a given measurement is not available, the corresponding 
state estimate has a greater error covariance than before. Similarly, available data reduces 
the error covariance appropriately based on the assigned standard deviation of that 
particular sensor output. 



3.7.4 Incorporating Fading Memory 

One problem with the standard Kalman implementation is filter “laziness”. In a 
completely observable system, the Kalman gain approaches zero as time goes to infinity. 
Therefore, the filter ignores new data since it believes that it has essentially perfect 
knowledge of all state variables. Changes in those variables caused by vehicle operations, 
such as a change in course or velocity, are not reflected in the filter. 
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Gelh provides a simple solution to this problem using recursive lading memory 
filtering. To force the filler to continue using new data, the error covariance of the 
predicted state vector is artificially increased (Gelb, 1974). This is accomplished with a 
simple addition to the algorithm for calculating P (t + 1 |r) . Now, the equation is: 

P(r+l|r) = sxtyxP(t\t) xf + rxQxr, 

with the only change being the addition of the s term, which is the fade factor. This is 
calculated by 



.v = exp (A t/x) , 



where 

At is the measurement interval (0.5 s), 
x is the age-weighting time constant. 

For this filler, the time constant is chosen to be 30 s to ensure a filter responsive to rapid 
changes in vehicle parameters while still retaining sufficient memory. The resulting fade 
factor is 1.0168. 

As Sorenson and Sacks explain (Sorenson and Sacks , 197 1), the use of this fade 
factor does not affect the stability of the Kalman filter. The only disadvantage is that by 
using the fade factor, the error covariance matrix is altered. Therefore, the error 
covariance matrix is no longer the best estimate of the error in the state estimate but only 
an approximation. 



3.8 Observability 

If a system is completely observable, the state vector can be entirely determined 
from the measurements. If the system is only partially observable, then any system errors 
involving unobservable quantities that propagate through the filter cannot be identified by 
measurements. For example, if position is not observed, then any consistent errors in 
velocity measurements will continue to increase the position error. In contrast, if velocity 
is not measured but position is, velocity can still be ohserved due to its known effect on 
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position. In the context of vehicle navigation, a partially observable system means that 
there is no way to provide a reasonable estimate of the unobservable quantities. If that is 
the case, the filter will be useless. 

To be observable, the matrix [C r \A r C T \(A T ) 2 C T \. . .1 (A r ) n ' l C T \ must have rank n 
(Gelb, 1974). When all data are available, the C T matrix has 17 linearly independent 
columns'by inspection, and the system is trivially observable. However, when fewer data 
are available the observability changes. When only IMU package, attitude package, and 
doppler velocimeter information is available, the observability matrix is no longer trivial. 
Powers of A t greater than two yield only zero matrices and do not contribute to 
observability. Eliminating zero columns and repeated columns. 



[C T \A T C T \A T2 C T ] = 



1 


0 


0 


0 


0 


0 


0 


0 


0 


0 


1 


0 


0 


0 


0 


0 


0 


0 


0 


0 


1 


0 


0 


0 


0 


0 


0 


0 


0 


0 


1 


0 


0 


0 


0 


0 


0 


0 


0 


0 


1 


0 


0 


0 
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0 
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0 


0 


0 
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0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 


0 



0 0 0 0 0 0 y 

0 0 0 0 0 0 A 

0 0 0 0 0 0 5 

0 0 0 0 0 a 0 

0 0 0 0 0 (3 0 

0 o o o o r o 

0 0 0 0 0 0 0 

0 0 0 0 0 0 0 

0 0 0 0 0 0 0 

0 0 0 0 0 0 0 

0 0 0 0 0 0 0 

1 0 0 0 0 0 0 

0 1 0 0 0 0 0 

0 0 1 0 0 0 0 

0 0 0 1 1 0 0 

0 0 0 -1 0 0 0 

0 0 0 0 -1 0 0 



The Greok letters a, P, T, y, A, and 5 represent elements of the 3x3 coordinate 
transformation matrix. In this case, they do not add to the rank ot the matrix since column 
15 containing them is simply a linear combination of columns four through six, while 
column 16 is a linear combination of the first three columns. Therefore, the rank of the 
matrix is only 14, leaving three state variables as unobservable. Not surprisingly, two ot 
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the unobservable states in this case are X- and Y-positions. Without observing position, 
there is no way to determine if the estimates of velocity and attitude contain errors causing 
divergence of the state estimates from the actual values. The third unobservahle state 
concerns gyro and compass bias. Without position measurements, true heading 
measurements are impossible. While independent measurements of gyro and magnetic 
compass headings allow the filter to observe the difference between the two biases, 
without true heading measurements the actual value of either cannot he determined (since 
the difference can be calculated, knowing one of these biases would permit calculation of 
the other. Thus there is only one additional unobservahle state due to the lack of true 
heading information). 

Similar calculations can be performed for the other two possible data 
combinations. Lacking only velocity data, the system is observable since position 
measurements can verify the accuracy of velocity estimates. Without X- and Y-position 
information available at least occasionally, the filter cannot be expected to provide valid 
estimates. 
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Chapter 4 

Kalman Filter Performance Using 
Simulated Data 

4.1 Purpose of Simulation 

The initial tests of this Kalman filter are performed using simulated data. This 
allows simple modification of data parameters to evaluate filter performance under a 
variety of conditions. Several tests are performed using the same basic data set. The 
specific parameters for each test are explained in detail. 



4.2 Data Generation 

4.2.1 Method of Data Generation 

The total time of the simulation is fixed at one hour. This provides ample time for 
filter evaluation and coincides with the period used for the actual data evaluation 
conducted in Chapter 5. 

Generating a valid data set is a multistep process. First, the control inputs are 
determined using reasonable values for linear accelerations and angular velocities. Then 
the nominal standard deviations for the control input measurements and system 
measurements are computed, along with system initial conditions. These use the values 
determined in Section 3.5, unless otherwise noted. For all simulated data, initial 
conditions are found in Table 4-1. 

To generate noise-free measurements in the appropriate coordinate system for each 
\ 

sensor, first the attitude measurements are computed for the entire hour using the 
MATLAB function “dlsim,” with the angular velocity measurements as input. Using these 
noise-free attitude computations, the vehicle-referenced linear accelerations are 
transformed into earth-referenced coordinates using the coordinate transformation found 
in Section 3. 7. 2. 2. Next, “dlsim” is used again to provide earth-referenced velocities and 
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Table 4-1: Initial Conditions for Simulations 



Parameter 


Initial Value 


X- Velocity 


0 


Y- Velocity 


0.5 m/s 


Z- Velocity 


0 


X-Position 


0 


Y-Position 


0 


Z-Position 


0 


Roll 


0 


Pitch 


0 


Heading 


300° 


Gyro Bias 


0 


Compass Bias 


-18° 



positions. Finally, attitude is used to transform the computed velocities into the vehicle- 
referenced coordinates used in the filter. 

After these noise-free measurements are computed, noise is added using the 
MATLAB function “randn,” which provides a Gaussian distribution with the desired 
variance. Additionally, the frequency of data output from each sensor is determined. The 
last step is to add desired gyro bias, compass bias, and any gyro drift. 

4.2.2 Noise-free Data Used for All Simulation Tests 

To provide a convenient basis for comparison among the tests, the same noise-free 
data set is used for each. Figures 4- 1 through 4-4 provide a graphic display of a sample of 
these measurements. 
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Fig. 4-1. Noise-free measurement of Y-velocity. 




Fig. 4.2. Noise-free measurement of Y-position. 
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Fig. 4-3. Noise-free measurement of vehicle pitch. 
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X Position in Meters Referenced to xinit 



Fig. 4-5. Comparison of measured and estimated X-Y position. 



4.3 Simulation Tests 

4.3.1 Test 1: Accurate Initial Conditions 

For the first test, doppler velocimeter readings are provided every 10 s and X-Y 
position information every 100 s. The initial conditions provided for the state variables 
match their actual values, including gyro and magnetic compass biases. Additionally, a 
gyro drift of 5°/hr is simulated. 

Figure 4-5 shows a comparison of actual to estimated position. Position data are 
available frequently, allowing the filter to maintain a close match between estimated and 
actual positions. Figure 4-6 provides a close-up of a smaller part of the data to allow better 
resolution. 

Estimating parameters with frequent data updates is relatively easy for the filter to 
accomplish. For example, Fig. 4-7 shows a portion of the data set comparing actual and 
estimated Z-position; Fig. 4-8 shows the same for roll. Heading is a more difficult state to 
estimate due to the inherent biases of the available instruments, the gyro and the compass. 
Figure 4-9 shows the estimate of heading throughout the data inn; Fig. 4-10 shows a 
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Z Position in Meters Referenced to zinit Y Position in Meters Referenced to yinit 
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Fig. 4-6. Close-up ot'X-Y Position Comparison. 
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Fig. 4-7. Comparison of actual and estimated Z-position. 
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Estimated Heading in Degrees 




Fig. 4-8. Comparison of actual and estimated roll. 




Fig. 4-9. Comparison of actual and estimated heading. 
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Fig. 4-10. Actual and estimated gyro bias. 

comparison of actual and estimated biases for the gyro. With the frequent availability of 
data, the filter provides a good estimate of the heading and associated biases. Overall, with 
Gaussian noise and frequent data the filter performs well. Subsequent tests check its 
performance under more strenuous conditions. 

4.3.2 Test 2: Inaccurate Initial Conditions Combined with Position Data 
Gap 

For the second test, all parameters are the same except for those noted in Table 4-2. 
Additionally, no position information is available for a 500-s interval towards the start of 
the run.. 



Table 4.2: Initial Conditions for Simulation Test 2. 



Parameter 


Initial Estimate 


Actual Value 


Compass Bias 


-24° 


-18° 


Gyro Bias 


-7° 


0° 


Heading 


307° 


300° 
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Fig. 4-11. Comparison of measured and estimated X-Y positions. 



As can be seen in Fig. 4-11, the lack of early position error eombined with the 
incorrect initial heading and biases causes some inaccuracies during the time without 
position data. However, after position information is again received the filter quickly 
adjusts to correct itself. 

The large error in the initial heading estimate is somewhat artificial since in actual 
operation, the towed vehicle should normally be within a few degrees of the surface ship’s 
average heading. However, even with a large initial error heading, gyro bias, and compass 
bias all approach their actual values by the end of the hour, as seen in Figs. 4-12 through 
4-14. If position information had been available throughout the data run, the final 
estimates would have been even closer. 

4.3.3 Test 3: Estimation Without Doppler Velocimeter 

The last test uses exactly the same parameters as test 2, except that no doppler 
velocimeter information is included. Under these conditions, an estimate of the , 
importance of the velocimeter can be made. The same graphs are produced for 
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• Gyro Bias in Degrees _ Estimated Heading in Degrees 




Fig. 4-12. Heading estimate. 




Fig. 4-13. Estimated and actual gyro bias. 
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Fig. 4-14. Estimate of magnetic compass bias. 



comparison. As can be seen in Figure 4-15, the gap in position data combined with the 
lack of accurate velocimeter information creates such a large error in position due to 
velocity errors that all subsequent fixes are rejected by the filter algorithm. This renders 
position unobservable, and filter divergence results. However, since the compass and gyro 
are still producing independent sources of heading information, the filter is able to make 
some corrections to bias. Without fixes to provide true heading measurements, the heading 
does not converge to its actual value but uses a weighted average of the gyro and compass 
measurements to minimize the error. 
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Fig. 4-16. Estimated heading. 
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Fig. 4-17. Estimated magnetic compass bias. 




Fig. 4-18. Comparison of actual and estimated gyro bias. 
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Chapter 5 

Kalman Filter Performance Using Actual 
Data 

5.1 Description of Data 

The data used for this evaluation were obtained in July, 1994, during operation of 
the DSL- 120, a towed vehicle mounting a sidescan sonar. Both data sets used for filter 
testing comprise approximately one hour of navigational sensor information. Due to the 
specific characteristics of the data package, two modifications were made to the Kalman 
filter algorithm to allow proper processing. 

First, gyro data were not obtained. Therefore, only the magnetic compass is used 
as the heading sensor for this data. As a consequence, the order of the filter is reduced by 
one due to the removal of gyro bias as a state variable. 

Second, analysis of the data reveals some anomalies. During the first data run, 
numerous spurious velocities of approximately 30 m/s are observed. In the second data 
run, regular spurious compass readings of exactly 40° are found. These anomalies are 
removed before using the data in the filter. 



5.2 First Data Set 

On initial inspection, the first data set appears to be excellent for Kalman filter use 
due to the frequency of obtained data, especially from the long-baseline positioning 
system. However, closer analysis reveals that the behavior of the linear accelerometers is 
erratic. Figure 5-1 shows an example of this behavior. The difference between the two 
concentrations of outputs is 0.08 g, which corresponds to a linear acceleration of 
approximately 0.8 m/s - . If the filter uses this erratic accelerometer data, it rapidly diverges 
due to the large estimated accelerations translating into large, nonexistent velocities. 
Therefore, to run the filter on this data set the accelerometer measurements are set to zero 
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Fig. 5.1. Output of Linear Accelerometer. 



for the entire run. The Kalman fi Iter uses the remaining measurements of velocity, 
position, roll, pitch, and heading to estimate all parameters. 

Figures 5-2 and 5-3 show the difference in measured and estimated X-Y positions. 
The doppler velocimeter provides accurate measurements of velocities, which 
compensates for the lack of accelerometer measurements. Therefore, the filter is able to 
track position well. 

As an example of a parameter which has the advantage of continuous 
measurements, a comparison of measured and estimated roll is shown in Fig. 5-4. The 
discrete character of the sensor measurements compared to the continuous Kalman filter 
estimate is obvious. 

The last estimate meriting special attention is heading. With the frequent fixes, true 
heading measurements are readily available. As can be seen from Figs. 5-5 and 5-6, 
occasionally an inaccurate true heading measurement will temporarily affect the estimated 
heading and bias. 
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5.2. Comparison of estimated and actual X-Y positions. 
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Fig. 5.3. Close-up of X-Y position comparison. 
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Fig. 5.4. Comparison of measured and estimated roll. 




Fig. 5.5. Estimated heading. 
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Fig. 5.6. Estimated compass bias. 



5.3 Second Data Set 

The second data set poses more of a challenge for the filter, since after four initial 
fixes are obtained there is a gap of nearly one-half hour without further XY-position 
information. However, the linear accelerometers do not show the same erratic behavior 
during this data set and are therefore used as part of the measurement vector. Figures 5-7 
through 5-12 show the performance of the filter in estimating position in light of this data 
gap. When position information is restored, there is a discontinuity in the filtered estimate 
of position due to the error developed by the filter estimate during the gap. After frequent 
fix information ts again received, filter position estimates track with actual positions 
obtained from the long-baseline navigation system. 

Besides position, the most important vehicle parameter to estimate accurately is 
heading. Without an accurate heading, sidescan sonar information will be inaccurate as 
well. Since both the gyro and the compass can be biased, a vital characteristic of the filter 



82 




Fig. 5.7. XY-position estimates (:) compared to position fixes (o). Note the presence of a 
flyer in the upper left corner, which was properly rejected by the filter. 



is its ability to estimate heading. Figure 5-112 shows the estimated heading during the 
hour. To understand filter behavior in the vicinity of this discontinuity, X-position 
behavior is examined individually in Fig. 5-10. After a lix is received following the long 
data gap, the first fix is weighted heavily and the estimated X-position jumps to the 
location of the fix. As can be seen in the figure, however, that first fix was rather 
inaccurate, although still within the specification of the fix rejection filter. Subsequent 
fixes are weighted less heavily since the error covariance matrix is adjusted after receipt of 
the first fix. Therefore, estimated position adjusts to actual position over several fixes. 

The jump in estimated heading at t=2000 seconds is a result of the gap in position 
information. However, despite this lengthy gap, the estimated heading remained 
consistent within three degrees. 

Parameters with more frequent data update rates are easier for the filter to estimate 
since measurements are continuously matched against the estimates. As one example, the 
comparison of estimated Z-position for a small portion of the hour is shown in Figure 5- 
12 . 
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Fig. 5.8. Comparison of estimated and actual vehicle track (blowup of Figure 5-7 with the 

region of the flyer excluded). 
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Fig. 5.9. Close-up of region of discontinuity following gap in position information. 
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Fig. 5.10. X-position behavior in the vicinity of the discontinuity. 
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Fig. 5.12. Comparison of measured and estimated Z-position. 



With the aid of the doppler velocimeter, the Kalman filter is able to maintain a 
valid estimate of vehicle parameters during this lengthy gap in position information. The 
key requirements in achieving this are: 

1. The doppler velocimeter is available to provide accurate velocity information 
throughout the data gap. 

2. Prior to losing position information, heading accuracy was sufficient to allow the 
Kalman filter to bridge the gap while still remaining within the window for fix 
acceptance once position data is regained. 
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Chapter 6 

Conclusions 



6.1 Fulfillment of Objectives 

The objective of this thesis was to develop an extended Kalman filter that could 
provide accurate estimates of underwater vehicle position and attitude in real-time. Based 
on the tests of the filter using both simulated and actual data, I believe that the result is a 
qualified success. On the positive side, the Kalman filter was able to process the data fast 
enough to permit real-time processing onboard ship, which would allow major reductions 
in post-cruise processing costs. Additionally, the real-time processing allows those 
conducting sidescan surveys to verify that the desired areas have been mapped by 
checking the navigational estimates. 

A second success is that when provided with sufficiently accurate and frequent 
data, the Kalman filter can provide accurate estimates of all vehicle motion and attitude 
changes. Even when gaps occur in the positioning system, the accuracy of the doppler 
velocimeter combined with a good prior estimate of heading allows the filter to maintain 
sufficient accuracy to arrive close to measured position when position data is regained. 
Also, the Kalman filter has the ability to estimate gyro bias and compass bias to 
compensate for inaccurate initial heading information and the effects of gyro drift. 
Therefore, the heading estimate becomes increasingly accurate as fix information is 
received and the filter is allowed to update over longer periods of time. 

In contrast to the successes, there were also some failures. Many of these can be 

attributed to sensor limitations, especially those of the linear accelerometers contained 

within the IMU package. A constant bias can be compensated for, but in the first data run 
1 , 

there was an example of erratic accelerometer behavior that threatened the viability of the 
entire estimation process. Only by recognizing this problem and relying on the doppler 
velocimeter entirely could accurate estimates of vehicle position be maintained. While in 
principle the Kalman filter is designed to permit any measurement to be used despite its 
noise characteristics, this application tests that theory due to the varying data rates of the 
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sensors. Fora “standard” Kalman filter, all measurements are obtained at every filter 
update step. Therefore, noisy measurements can be made nearly meaningless by assigning 
greater inherent accuracy to less noisy ones. In this case, that is not possible. When the 
only available measurement in the series of integrations leading to position is acceleration, 
the filter is forced to use it. Even though its weight is initially low, over time these 
measurements have a significant effect on the estimates of velocity and position until other 
sensor data are received. 

A second concern is whether the heading estimates provided by the filter are 
accurate enough to be truly useful for sidescan sonar mapping operation. When scanning 
at long range, even a few tenths of a degree of heading error can be significant. While 
heading accuracy should continue to improve as the filter operates for longer periods, a 
compromise is always necessary when deciding how to weight old measurements. If old 
measurements are weighted too heavily, the filter does not respond to actual changes in 
state variables, such as a change in course. On the other hand, weighting old 
measurements less results in greater filter movement toward new measurements with 
subsequent instability and potentially erroneous changes in bias estimates. 

Finally, the filter as presently constructed is of limited utility in estimating vehicle 
motion for a maneuvering vehicle. While, theoretically, control inputs could he added 
fairly easily, the modeling necessary for accurate results is not yet a reality. While most 
current operations involving a thruster-controlled vehicle are limited to a small area and 
can therefore take advantage of a high update rate for position information, future 
advances in vehicle technology promise a long-range, independently-operating 
autonomous vehicle. As constructed here, it is doubtful that the Kalman filter can provide 
sufficient accuracy given the sensors available. 



6.2 Future Work 

There is much work that could be done to improve the performance of this Kalman 
filter estimation process. A few possibilities are as follows. 

First and most obviously, improved sensors would translate directly into improved 
measurements. The primary candidates for such improvements are the linear 



accelerometers and the gyro. Without gyro stabilization, it is difficult to improve 
accelerometer stability. However, improvements continue to be made, and it is hoped that 
cost, weight, and power requirements will continue to decline to allow better acceleration 
measurements. As an adjunct to this effort, more testing needs to be performed on the 
accelerometers in use to understand their characteristics and limitations more fully. 
Hopefully, the ring-laser gyro will soon become inexpensive enough for practical use in 
this application to improve heading measurements. Also, the method of obtaining true 
heading used here is only one possibility. Any improvements in true heading accuracy 
would improve the estimates of both heading and heading sensor bias. 

To allow this filter to be useful for such highly maneuverable vehicles as Jason , 
further work needs to be earned out on modeling control inputs to the filter. Additionally, 
if a strapdown IMU package is still in use, the addition of the effects of angular velocities 
on linear acceleration and velocities may have to be considered due to the greater angular 
velocities involved. 

Finally, there are always refinements which can be made to improve filter 
performance. One possibility is to add to the state vector estimates for other sensor biases. 
This has the potential for improving all measurements, especially those of the 
accelerometers, which appear to have biases that are always present and frequently 
changing. 
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Appendix 

KFSetup Program 



% This program provides the initial conditions required to run the Kalman 
% filter algorithm. 

DTOR = pi/ 180;% Conversion of degrees to radians 

% The A matrix propagates the state variables 
A = zeros(17); 

A(4:6,l:3)=eye(3); 

A(13: 15,10: 12)=eye(3); 

% The G matrix provides the effects of inputs (accelerations and angular 
% velocities) on the state variables 

G = zeros(17,6); 

G(l:3,l:3) = eye(3); 

G( 10: 12,4:6) = eye(3); 

% The C matrix is the measurement matrix 

CHDG = [1 -1 0;1 0 - 1 ; 1 0 ()]; % 3x3 matrix used to form heading portions of C matrices 
CF = eye(17);% Full C matrix when all info available 
CF(15: 17,15: 17) = CHDG; 

CNV = zeros(14,17);% C matrix when no velocity info available 
CNV(1:3,1:3) = eye(3); 

CNV(4:1 1,7:1 4)=eye(8); 

CNV(12:14, 15:17) = CHDG; 

CNP = zeros(14,17);% C matrix when no position info available 
CNP(l:6,l:6) = eye(6); 

CNP(7: 12,9: 14) = eye(6); 

CNP(13:14, 15:17) = CHDG(l:2,:); 

CNPV = zeros(l 1,17);% C matrix when neither position nor velocity available 
CNPV(1:3,1:3) = eye(3); 

CNPV(4:9,9:14\=eye(6); 

CNPV(10:11, 15:17) = CHDG(1:2,:); 

% The D matrix is necessary only for generating simulated data 
D = zeros(17,6); 

DNV = zeros(14,6); 

DNP = zeros(14,6); 

DNPV = zeros(l 1,6); 

% EiTor Covariance Matrix for random forcing function 
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Q = zeros (6); 

QXYZ = (0.2*eye(3)). A 2; % Set variance for accelerometers 
Q( 1:3, 1:3) = QXYZ; 

QRPH = (2*DTOR*eye(3)). A 2; % Set variance for angular velocimeters 

Q(4:6,4:6) = QRPH; 

% Error Covariance Matrix for measurements 
RACC = (0. 1 *eye(3)). A 2; 

RAR = (0.5*DTOR*eye(3)). A 2; 

RPos = (3*eye(2)). A 2; % Set variance for acoustic position fixes 
RDepth = 0.1 A 2;% Set variance for depth 

RPR = (0.5*DTOR*eye(2)). A 2; % Set variance for pitch and roll 
RHG = (0. l*DTOR) A 2;% Set variance for heading by gyro 
RHM = (,5*DTOR) A 2; % Set variance for heading by compass 

% Reference Points 
xinit = 0; 
yinit = 0; 
tinit = 0; 
zinit = 0; 



magvar = -24; % Set initial magnetic compass bias 
gyrovar = -7 ; % Set gyro bias 
% Initial conditions 
x_()ACC = [();();()]; 
x_()AR = [();();()]; 

x_0V = [0;0.5;0];% Velocities (x,y,z) 

xJIXY = [0-xinit;()-yinit] ; % Position (x,y) 

x_()Z = zinit-0;% Position (z) 

x_()R = ()*DTOR;% Roll 

x_()P = 0*DTOR;% Pitch 

x_()H = (3(H)+gyrovar)*DTOR;% Heading 

x_() = 

[x_0ACC;x_()V;x_0XY ;x_()Z;x_()AR;x_()R;x_()P;x_()H;gyrovar*DTOR;magvar*DTOR] 



% Initial Error covariance matrix 

P_0 = zeros(17); 7 « Initial P matrix for states 

P_( )( 1 : 3 , 1 : 3 ) = (0.1*eye(3)) A 2; 

P_0(4:6,4:6) = (,02*eye(3)). A 2; % Initial uncertainty in velocity (m/s) 
P_0(7:8,7:8) = (3*eye(2)). A 2; % Initial uncertainty in position (m) 

P_0(9,9) = 0.3 A 2; % Initial uncertainty in depth (m) 

P_()( 10: 12, 10: 12) = (0.5*DTOR*eye(3)). A 2; 

P_()( 1 3: 14, 1 3: 14) = (l*DTOR*eye(2)) A 2; % Initial uncertainty in roll and pitch 

P ()( 15,15) = (2*DTOR) A 2;% Initial uncertainty in heading 

P_()( 16, 16) = (2*DT()R) A 2;% Set initial gyro bias variance 

P_( )( 1 7 , 1 7 ) = (2*DTOR) A 2;% Set initial magnetic compass bias variance 
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w = zeros(6,l);% random forcing function 
g = -9.8;% gravity vector 

% Set IMU biases to be subtracted from raw data (units of g for acc, degrees/s for 
% angular rates) 



xaccbias = 0; 
yaccbias-= 0; 
zaccbias = 0; 
rrbias = 0; 
prbias = 0; 
yrbias = 0; 
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KFRun 



%dt = 1/24/3600/2; % Gives desired kalman filter update time step of 0.5 seconds 
tO = tinit; % Set initial time for filter for calculating time steps 
t = tO; • 

tfix = tO; % Time of last fix 

kk = 1; % Index for state estimate file 

nn = 0; 

mm = 0; 

qq = 1; 

zl = zeros(3,l); 
z2 = zeros(3,l); 

Posmat = zeros(4,2); 
fixcount = 0; 

SWAcheck = ();% Set Counters to count lines of each type of data in filter step 
PAScheek = 0; 

PVSeheck = 0; 

LBLcheck = 0; 

FixedSim2=zeros(7200,18); % Matrix to hold processed raw data 
Templn=[]; 

EstSim2 = []; 

rejfix = 0; % Counter to keep track of rejected fixes 
Xp = x_0 ; % Set initial condition for estimated states 
Pp = P_0; % Set initial condition for error covariance matrix 
while nn < 7200; % Setup loop to read all data from file 

% The following loop ensures both SWA and PAS data are available to the filter 
while SWAcheck == 0 I PAScheek == 0 
mm = mm+1; 
nn = nn+1; 

Templn(mm,:) = SIM(nn,:); % SIM is data file 
Templn(mm,l) = Templn(mm,l)-tinit; 

[M,N] = size(TempIn); 
t = TempIn(M,l); 

% This loop reads the next dt worth of data from the file. For real-time 
% processing, use same principle to read dt worth of data into Templn. 
while Templn(mm, l)-t() <= dt; % Check to see if have dt worth of data 
nn = nn+1; 
mm = mm+1; 

Templn(mm,:) = SIM(nn,:); 

Templn(mm,l) = Templn(mm,l)-tinit; 

[M,N] = size(TempIn); 
t = TempIn(M,l); 
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end 



% This loop cheeks if both SWA and PAS data is present. If the data does not 
% contain at least one sample of both SWA and PAS data, another dt worth will 
% be added prior to proceeding, 
for i = qq:M 

if Templn(i,2) <10631 I Templn(i,2) > 10633 

SWAcheck = SWAcheck+1; 

else 

Templn(i,2:7) = zeros(l,6); % set bogus data to zero 
end 

if Templn(i,13) <10631 I Templn(i,13) > 10633 
PAScheck = PAScheck+1; 

Templn(i,13) = zinit-TempIn(i,13); 

Templn(i, 1 4: 15) = Templn(i,14: 15).*DTOR; 

if (360-TempIn(l,16))<75 I Templn(l,16)<75 

if Templn(i,16)>270 

Templn(i, 16)=TempIn(i, 1 6)-360; 

else 

end 

else 

end 

Templn(i,16) = TempIn(i,16)*DTOR; 

if (360-TempIn(l,17))<75 I Templn(l,17)<75 

if Templnfi, 17)>270 

TempIn(i,17)=TempIn(i,17)-360; 

else 

end 

else 

end 

Templn(i,17) = TempIn(i,17)*DTOR; 
else 

Templn(i,13: 17) = zeros(l,5); % set bogus data to zero 
end 

if Templn(i,8) <10631 I Templn(i,8) > 10633 
PVScheck = PVScheck+1; 
else 

Templn(i,8: 10) = zeros(l,3); % set bogus data to zero 

Templn(i,18) =(); 

end 

if Templn(i,l 1) <10631 I Templn(i,l 1) > 10633 
LBLcheck = LBLcheck+1; 

Templn(i,ll) =TempIn(i,l l)-xinit; 

Templn(i,12) = Templn(i,12)-yinit; 
else 

Templn(i,l 1:12) = zeros(l,2); % set bogus data to zero 



% 



end 

end 

qq = M+l; 
end 

FixedSim2((nn+l-M):nn+l-l,:)=TempIn; 

% Now we will provide the estimates of x A (t+llt) and P(t+llt). These are done 
% now rather as the last step in the filter propagation due to the variable 
% time step. Now that we know the time step, we can more accurately project 
% the step estimates into the future. 

%DT = (t-t())*24*3600; % This is the length of the current filter time step. 

[Xm,Pm] = fullkfpred(A,G,w,Q,Xp,Pp,DT); 

% We now have enough data to run the filter. For data with more than one time 
% sample’s worth present, the data for the whole time step is averaged. 

% Also, any suspected bias is removed. The inputs are converted into the proper 
% units for filter use. Also, the data is checked to see if LBL and/or doppler 
% data is available, which will determine which version of the filter to use. 

% Therefore, the input to the kalman filter will have only one line of 
% averaged data, which will always include SWA and PAS data, and may also 
% have LBL and/or doppler data. 

zl ( 1 ) = (((sum(TempIn(:,2))/SW Acheck))-xaccbias)*(-g)+((sin(Xp(l 3)))*g); 
zl(2) = (((sum(TempIn(:,3))/SWAcheck))-yaccbias)*(-g)-((sin(Xp(14)))*g); 
z 1 (3) = (((sum(TempIn(:,4))/SWAcheck))-zaccbias)*(-g)- 

((cos(Xp( 1 3))*cos(Xp( 1 4)))*g); 

z2(l) = (((sum(TempIn(:,5))/SWAcheck))-rrbias)*DTOR; 
z2(2) = (((sum(TempIn(:,6))/SWAcheck))-prbias)*DTOR; 
z2(3) = (((sum(TempIn(:,7))/SWAcheck))-yrbias)*DTOR; 

%zl=zeros(3,l); % Used only if not using actual accelerometer measurements 
%z2=zeros(3,l); 

% One other requirement is to check for position flyers. If a position update 
% is bogus, it will be ignored and the filter run without it. A position update 
% will be compared with the current filter estimate of position. If they differ 
% by more than five times the std deviation of the position uncertainty, the 
% position update will he ignored, 
if LBLcheck ~= 0 
tcheck = (t-tfix); 

Pcheckx = sum(TempIn(:,l l))/LBLcheck; 

Pchecky = sum'(TempIn(:,12))/LBLcheck; 

errest = sqrt(((Pcheckx-Xp(7)) A 2)+(Pchecky-Xp(8)) A 2); 

if errest > (10*RPos(Ll))+(0.()5*tcheck) 

rejfix = rejfix+1; 

LBLcheck = 0; 

else 

tfix = t; 
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end 

end 



% The first case is when all data is available. True heading will only be 
% available after five fixes have been obtained. 

% Angular measurements have already been converted to radians using DTOR. 
if LBLcheck ~= 0 & PVScheck ~= 0 
fixcount = fixcount+1; 

Z = zeros(17,l); 

Z(l:3) = zl; 

Z(4) = sum(TempIn(:,8))/PVScheck; 

Z(5) = sum(TempIn(:,9))/PVScheck; 

Z(6) = sum(TempIn(:,10))/PVScheck; 
xLBL = sum(TempIn(:,l l))/LBLcheck; 

Z(7) = xLBL; 

yLBL = sum(TempIn(:,12))/LBLcheck; 

Z(8) = yLBL; 

Z(9) = sum(TempIn(:,13))/PAScheck; 

Z(10:12) = z2; 

Z(13) = sum(TempIn(:,14))/PAScheck; 

Z(14) = sum(TempIn(:,15))/PASeheck; 
ghdg = sum(TempIn(:,16))/PAScheck; 
if ghdg<0 

ghdg = ghdg+(2*pi); 

else 

end 

Z(15) = ghdg; 

mhdg = sum(TempIn(:,17))/PAScheck; 
if mhdg<() 

mhdg = mhdg+(2*pi); 

else 

end 

Z(16) = mhdg; 

R = zeros(17); 

R( 1:3, 1:3) = RACC; 

R(4:6,4:6) = eye(3).*((sum(TempIn(:,18))/PVScheck) A 2); 

R(7:8,7:8) = RPos; 

R(9,9) = R Depth; 

R( 10: 12,10: 12) = RAR; 

R( 13: 14, 13: 14)'= RPR; 

R( 15,15) = RHG; 

R(16,16) = RHM; 

C = CF; 

% Update the matrix containing the last four position fixes 
Posmat(l,:) = Posmat(2,:); 
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Posmat(2,:) = Posmat(3,:); 

Posmat(3,:) = Posmat(4,:); 

Posmat(4,:) = [xLBL,yLBL]; 
if fixcount<4 
Z = Z( 1:16); 

R = R(1 : 16, 1 : 16); 

C = CF(1:16,:); 
else 

% Now, calculate true heading using a least-squares fit to the last lour fixes 
L = [l,Posmat(l,l);l,Posmat(2,l);l,Posmat(3,l);l,xLBL]; 

B = [Posmat(l,2);Posmat(2,2);Posmat(3,2);yLBL]; 

bfit = inv(L’*L)*L’*B; 

if bfit(2)>=0 & yLBL > Posmat(l,2) 

thdg = (pi/2)-atan(bfit(2)); 

elseif bfit(2)<0 & yLBL < Posmat(l,2) 

thdg = (pi/2)-atan(bfit(2)); 

else 

thdg = (3*pi/2)-atan(bfit(2)); 
end 

Z(17) = thdg; 

% Calculate variance of this heading measurement 
[HV] = hvar(Posmat,thdg); 

R( 17, 17) = [HV]; 
end 

% The second case is when SWA, PAS, and LBL data is available, but no doppler. 



elseif LBLcheck ~= 0 & PVScheck == 0 
fixcount = fixcount+1; 

Z = zeros(14,l); 

Z(l:3) = zl; 

xLBL = sum(TempIn(;,l l))/LBLcheck; 
Z(4) = xLBL; 

yLBL = sum(TempIn(:,12))/LBLcheck; 
Z(5) = yLBL; 

Z(6) = sum(TempIn(:,13))/PAScheck; 
Z(7:9) = z2; 

Z(10) = sum(TempIn(:,14))/PAScheck; 
Z( 11) = sum(TempIn(:, 15))/PAScheck; 
ghdg = sum(TempIn(:,16))/PAScheck; 
if ghdg<() 

ghdg = ghdg+(2*pi); 

else 

end 

Z(12) = ghdg; 
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mhdg = sum(TempIn(.\ 17))/PAScheck; 
if mhdgcO 

mhdg = mhdg+(2*pi); 

else 

end 

Z(13) = mhdg; 

R = zeros(14,14); 

R( 1:3, 1:3) = RACC; 

R(4:5,4:5) = RPos; 

R(6,6) = RDepth; 

R(7:9,7:9) = RAR; 

R(10:l 1,10:11) = RPR; 

R( 12, 1 2) = RHG; 

R(13,13) = RHM; 

C = CNV; % Update the matrix containing the last four position fixes 
Posmat(l,:) = Posmat(2,:); 

Posmat(2,:) = Posmat(3,:); 

Posmat(3,:) = Posmat(4,:); 

Posmat(4,:) = [xLBL,yLBL]; 
if fixcount<4 
Z = Z(l:13); 

R = R( 1:13,1:1 3); 

C = CNV(1:13,:); 
else 

% Calculate true heading 

L = [ 1 ,Posmat( 1 , 1 ); 1 ,Posmat(2, 1); 1 ,Posmat(3, 1 ); 1 ,xLBL] ; 

B = [Posmat(l,2);Posmat(2,2);Posmat(3,2);yLBL]; 
bfit = inv(L’*L)*L’*B; 
if bfit(2)>=() & yL,BL > Posmat(l,2) 
thdg = (pi/2)-atan(bfit(2)); 
elseif bfit(2)<0 & yLBL < Posmat(l,2) 
thdg = (pi/2)-atan(bfit(2)); 
else 

thdg = (3*pi/2)-atan(bfit(2)); 
end 

Z(14) = thdg; 

% Calculate variance of this heading measurement 
[HV] = hvar(Posmat,thdg); 

R(14, 14) = [HVJ; 
end 

% The third case is when SWA, PAS, and doppler data is available, but no LBL. 

elseif LBLcheck == 0 & PVScheck -= 0 
Z = zeros(14,l); 

Z( 1:3) = zl; 
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Z(4) = sum(TempIn(:,8))/PVScheck; 

Z(5) = sum(TempIn(:,9))/PVScheck; 

Z(6) = sum(TempIn(:,l()))/PVScheck; 

Z(7) = sum (Tem pln(: , 1 3))/P AScheck; 

Z(8:10) = z2; 

Z(1 1) = sum(TempIn(:,14))/PAScheck; 

Z(12) = sum(TempIn(:,15))/PAScheck; 
ghdg = sum(TempIn(:,16))/P AScheck; 
if ghdg<() 

ghdg = ghdg+(2*pi); 

else 

end 

Z(13) = ghdg; 

mhdg = sum(TempIn(:,17))/PAScheck; 
if mhdgcO 

mhdg - mhdg+(2*pi); 

else 

end 

Z(14) = mhdg; 

R = zeros(14); 

R(l:3,l:3) = RACC; 

R(4:6,4:6) = eye(3).*((sum(TempIn(:,18))/PVScheck) A 2); 
R(7,7) = R Depth; 

R(8:l(),8: 10) = RAR; 

R(1 1:12,1 1:12) = RPR; 

R( 13,13) = RHG; 

R(14,14) = RHM; 

C = CNP; 

% The last case is when only SWA and PAS data is available, 
else 

Z = zeros(l 1,1); 

Z(l:3) = zl; 

Z(4) = sum(Templn(:,13))/PAScheck; 

Z(5:7) = z2; 

Z(8) = sum(TempIn(:,14))/PAScheck; 

Z(9) = sum(TempIn(:,15))/PAScheck; 
ghdg = sum(TempIn(:,16))/PAScheck; 
if ghdgcO 

ghdg = ghdg+(2*pi); 

else 

end 

2X10) = ghdg; 

mhdg = sum(TempIn(:,17))/PAScheck; 
if mhdg<() 
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mhdg = mhdg+(2*pi); 

else 

end 

Z( 11) = mhdg; 

R = zeros(l 1); 

R(l:3,l:3) = RACC; 

R(4,4) = RDepth; 

R(5:7,5:7) = RAR; 

R(8:9,8:9) = RPR; 

R(10,10) = RHG; 

R(1 1,1 1) = RHM; 

C = CNPV; 
end 

% Now the filter can finally be run using KFcalc. 

[Xp,Pp]= fullkfcalc(C,R,Xm,Pm,Z); 

EstSim2(kk,l:18) = [Xp’,t]; % Store state estimates with time stamp nn 
SWAcheck = 0; 

PAScheck = 0; 

LBLcheck = 0; 

PVScheck = 0; 
mm = 0; 
tO = t; 
qq = 1; 

Templn = []; 
kk = kk+ 1 ; 
pp=i; 

end 
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KFpred 



% The function kfpred provides the estimates of x A (t+llt) and P(t+llt). 

% It must be done out of the normal order for a Kalman filter since the time 
% step is unknown until the next block of data is processed. 

% The format is: [Xm,Pm]=kfpred(A,G,U,Q,xcurr,P,t) 

% t is the time stamp for the current update step. 
function[Xm,Pm]=kfpred(A,G,U,Q,xeurr,P,t) 

headg = xcurr(15); 
pitch = xcurr(14); 
roll = xcurr(13); 

dheadg = (2*pi - headg); % Change axes 

pcos = cos(pitch); 

psin = sin(pitch); 

rcos = cos(roll); 

rsin = sin(roll); 

hcos = cos(dheadg); 

hsin = sin(dheadg); 

A(7,4) = rcos*hcos; 

A(7,5) = psin*rsin*hcos - pcos*hsin; 

A(7,6) = pcos*rsin*hcos + psin*hsin; 

A(8,4) = hsin*rcos; 

A(8,5) = psin*rsin*hsin + pcos* hcos; 

A(8,6) = pcos*rsin*hsin - psin*hcos; 

A(9,4) = -rsin; A(9,5) = psin*rcos; 

A(9,6) = pcos*rcos; 

[Phi, Gamma] = c2d(A,G,t); 

Xm = Phi*xcurr + Gamma*U; % Xm = x A (t+llt) 

%Pm = 1.0168*Phi*P*Phi’ + Gamma*Q*Gamma’; % Use for maneuvering vehicle 
Pm = l.()()l*Phi*P*Phi’ + Gamma*Q*Gamma’; % Use when have position data gaps or 
no maneuvering 
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FullKFcalc 



% The function FullKFCalc implements the kalman filter for full data sets 
% The format is: [Xp,Pp]=fullkfcalc(C,R,Xm,P,Z) 

% t is the time stamp for the current update step. 

function[Xp,Pp]=fullkfcalc(C,R,Xm,P,Z) 

while Xm(15) < 0 

Xm(15) = Xm(15)+(2*pi); 

end 

while Xm(15) > 2*pi 
Xm(15) = Xm(15)-(2*pi); 
end 

while Xm(16)>pi 

Xm(16)=Xm(16)-(2*pi); 

end 

while Xm(16)<(-pi) 

Xm(16)=Xm(16)+(2*pi); 

end 

while Xm(17)>pi 

Xm(17)=Xm(17)-(2*pi); 

end 

while Xm(17)<(-pi) 

Xm(17)=Xm(17)+(2*pi); 

end 

Ze = C*Xm; % Ze = y A (tlt-l) 

I = Z - Ze; % I = y(t)-y A (tlt-l) 

v = C*P*C’+R; % v = covariance matrix of innovations 

k = P*C’*inv(v); 

Xp = Xm+k*I; % xp = x A (tlt) 

while Xp(15) < 0 
Xp(15) = Xp(15)+(2*pi); 
end 

while Xp(15) > 2*pi 
Xp(15) = Xp(15)-(2*pi); 
end 

while Xp(16)>pi 

Xp( 1 6)=Xp( 1 6)-(2*pi); 

end 

while Xp(16)<(-pi) 

Xp(16)=Xp(16)+(2*pi); 

end 

while Xp(17)>pi 
Xp(17)=Xp(17)-(2*pi); 
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end 

while Xp(17)<(-pi) 

Xp(17)=Xp(17)+(2*pi); 

end 

temp = eye(17)-(k*C); 

Pp = temp*P*temp’+k*R*k’ ; % p = P(tlt) 
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HVar 



% The function hvar provides the variance of the true heading 
% measurement. 

% The format is [HV] = hvar(Posmat,thdg) 
function[HV] = hvar(Posmat,thdg) 
tempvar = 0; 

Htemp = zeros(3,l); 
for i = 1:3; 

LL = [l,Posmat(i,l);l,Posmat(i+l,l)j; 

BB = [Posmat(i,2);Posmat(i+l,2)j; 
bbfit = inv(LL’*LL)*LL’*BB; 
if bbfit(2)>=0 & BB(2)>BB(1) 
temphdg = (pi/2)-atan(bbfit(2)); 
elseif bbfit(2)<0 & BB(2)<BB(1) 
temphdg = (pi/2)-atan(bbfit(2)); 
else temphdg = (3*pi/2)-atan(bbfit(2)); 
end 

Htemp(i) = temphdg; 
end 

for i = 1:3; 
if Htemp(i)-thdg>pi 
HVar(i) = ((Htemp(i)-(2*pi))-thdg) A 2; 
elseif thdg-Htemp(i)>pi 
HVar(i) = ((thdg-(2*pi))-Htemp(i)) A 2; 
else 

HVar(i) = (Htemp(i)-thdg) A 2; 

end 

end 

HV = (sum(HVar))/3; 
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